Imported upstream 4.91upstream-4.91upstreampackaging

46 files changed, 12763 insertions, 0 deletions

diff --git a/test/agent.c b/test/agent.c
new file mode 100644
index 0000000..a40039e
--- /dev/null
+++ b/test/agent.c
@@ -0,0 +1,700 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <getopt.h>
+#include <string.h>
+
+#include <dbus/dbus.h>
+
+static char *passkey_value = NULL;
+static int passkey_delay = 0;
+static int do_reject = 0;
+
+static volatile sig_atomic_t __io_canceled = 0;
+static volatile sig_atomic_t __io_terminated = 0;
+
+static void sig_term(int sig)
+{
+	__io_canceled = 1;
+}
+
+static DBusHandlerResult agent_filter(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	const char *name, *old, *new;
+
+	if (!dbus_message_is_signal(msg, DBUS_INTERFACE_DBUS,
+						"NameOwnerChanged"))
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+
+	if (!dbus_message_get_args(msg, NULL,
+					DBUS_TYPE_STRING, &name,
+					DBUS_TYPE_STRING, &old,
+					DBUS_TYPE_STRING, &new,
+					DBUS_TYPE_INVALID)) {
+		fprintf(stderr, "Invalid arguments for NameOwnerChanged signal");
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	}
+
+	if (!strcmp(name, "org.bluez") && *new == '\0') {
+		fprintf(stderr, "Agent has been terminated\n");
+		__io_terminated = 1;
+	}
+
+	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+}
+
+static DBusHandlerResult request_pincode_message(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	DBusMessage *reply;
+	const char *path;
+
+	if (!passkey_value)
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
+							DBUS_TYPE_INVALID)) {
+		fprintf(stderr, "Invalid arguments for RequestPinCode method");
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	}
+
+	if (do_reject) {
+		reply = dbus_message_new_error(msg, "org.bluez.Error.Rejected", "");
+		goto send;
+	}
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply) {
+		fprintf(stderr, "Can't create reply message\n");
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+	}
+
+	printf("Pincode request for device %s\n", path);
+
+	if (passkey_delay) {
+		printf("Waiting for %d seconds\n", passkey_delay);
+		sleep(passkey_delay);
+	}
+
+	dbus_message_append_args(reply, DBUS_TYPE_STRING, &passkey_value,
+							DBUS_TYPE_INVALID);
+
+send:
+	dbus_connection_send(conn, reply, NULL);
+
+	dbus_connection_flush(conn);
+
+	dbus_message_unref(reply);
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult request_passkey_message(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	DBusMessage *reply;
+	const char *path;
+	unsigned int passkey;
+
+	if (!passkey_value)
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
+							DBUS_TYPE_INVALID)) {
+		fprintf(stderr, "Invalid arguments for RequestPasskey method");
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	}
+
+	if (do_reject) {
+		reply = dbus_message_new_error(msg, "org.bluez.Error.Rejected", "");
+		goto send;
+	}
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply) {
+		fprintf(stderr, "Can't create reply message\n");
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+	}
+
+	printf("Passkey request for device %s\n", path);
+
+	if (passkey_delay) {
+		printf("Waiting for %d seconds\n", passkey_delay);
+		sleep(passkey_delay);
+	}
+
+	passkey = strtoul(passkey_value, NULL, 10);
+
+	dbus_message_append_args(reply, DBUS_TYPE_UINT32, &passkey,
+							DBUS_TYPE_INVALID);
+
+send:
+	dbus_connection_send(conn, reply, NULL);
+
+	dbus_connection_flush(conn);
+
+	dbus_message_unref(reply);
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult request_confirmation_message(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	DBusMessage *reply;
+	const char *path;
+	unsigned int passkey;
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
+						DBUS_TYPE_UINT32, &passkey,
+							DBUS_TYPE_INVALID)) {
+		fprintf(stderr, "Invalid arguments for RequestPasskey method");
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	}
+
+	if (do_reject) {
+		reply = dbus_message_new_error(msg, "org.bluez.Error.Rejected", "");
+		goto send;
+	}
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply) {
+		fprintf(stderr, "Can't create reply message\n");
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+	}
+
+	printf("Confirmation request of %u for device %s\n", passkey, path);
+
+	if (passkey_delay) {
+		printf("Waiting for %d seconds\n", passkey_delay);
+		sleep(passkey_delay);
+	}
+
+send:
+	dbus_connection_send(conn, reply, NULL);
+
+	dbus_connection_flush(conn);
+
+	dbus_message_unref(reply);
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult authorize_message(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	DBusMessage *reply;
+	const char *path, *uuid;
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
+						DBUS_TYPE_STRING, &uuid,
+							DBUS_TYPE_INVALID)) {
+		fprintf(stderr, "Invalid arguments for Authorize method");
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	}
+
+	if (do_reject) {
+		reply = dbus_message_new_error(msg, "org.bluez.Error.Rejected", "");
+		goto send;
+	}
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply) {
+		fprintf(stderr, "Can't create reply message\n");
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+	}
+
+	printf("Authorizing request for %s\n", path);
+
+send:
+	dbus_connection_send(conn, reply, NULL);
+
+	dbus_connection_flush(conn);
+
+	dbus_message_unref(reply);
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult cancel_message(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	DBusMessage *reply;
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_INVALID)) {
+		fprintf(stderr, "Invalid arguments for passkey Confirm method");
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	}
+
+	printf("Request canceled\n");
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply) {
+		fprintf(stderr, "Can't create reply message\n");
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+	}
+
+	dbus_connection_send(conn, reply, NULL);
+
+	dbus_connection_flush(conn);
+
+	dbus_message_unref(reply);
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult release_message(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	DBusMessage *reply;
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_INVALID)) {
+		fprintf(stderr, "Invalid arguments for Release method");
+		return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+	}
+
+	if (!__io_canceled)
+		fprintf(stderr, "Agent has been released\n");
+
+	__io_terminated = 1;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply) {
+		fprintf(stderr, "Can't create reply message\n");
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+	}
+
+	dbus_connection_send(conn, reply, NULL);
+
+	dbus_connection_flush(conn);
+
+	dbus_message_unref(reply);
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult agent_message(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	if (dbus_message_is_method_call(msg, "org.bluez.Agent",
+							"RequestPinCode"))
+		return request_pincode_message(conn, msg, data);
+
+	if (dbus_message_is_method_call(msg, "org.bluez.Agent",
+							"RequestPasskey"))
+		return request_passkey_message(conn, msg, data);
+
+	if (dbus_message_is_method_call(msg, "org.bluez.Agent",
+							"RequestConfirmation"))
+		return request_confirmation_message(conn, msg, data);
+
+	if (dbus_message_is_method_call(msg, "org.bluez.Agent", "Authorize"))
+		return authorize_message(conn, msg, data);
+
+	if (dbus_message_is_method_call(msg, "org.bluez.Agent", "Cancel"))
+		return cancel_message(conn, msg, data);
+
+	if (dbus_message_is_method_call(msg, "org.bluez.Agent", "Release"))
+		return release_message(conn, msg, data);
+
+	return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+}
+
+static const DBusObjectPathVTable agent_table = {
+	.message_function = agent_message,
+};
+
+static int register_agent(DBusConnection *conn, const char *adapter_path,
+						const char *agent_path,
+						const char *capabilities)
+{
+	DBusMessage *msg, *reply;
+	DBusError err;
+
+	msg = dbus_message_new_method_call("org.bluez", adapter_path,
+					"org.bluez.Adapter", "RegisterAgent");
+	if (!msg) {
+		fprintf(stderr, "Can't allocate new method call\n");
+		return -1;
+	}
+
+	dbus_message_append_args(msg, DBUS_TYPE_OBJECT_PATH, &agent_path,
+					DBUS_TYPE_STRING, &capabilities,
+					DBUS_TYPE_INVALID);
+
+	dbus_error_init(&err);
+
+	reply = dbus_connection_send_with_reply_and_block(conn, msg, -1, &err);
+
+	dbus_message_unref(msg);
+
+	if (!reply) {
+		fprintf(stderr, "Can't register agent\n");
+		if (dbus_error_is_set(&err)) {
+			fprintf(stderr, "%s\n", err.message);
+			dbus_error_free(&err);
+		}
+		return -1;
+	}
+
+	dbus_message_unref(reply);
+
+	dbus_connection_flush(conn);
+
+	return 0;
+}
+
+static int unregister_agent(DBusConnection *conn, const char *adapter_path,
+							const char *agent_path)
+{
+	DBusMessage *msg, *reply;
+	DBusError err;
+
+	msg = dbus_message_new_method_call("org.bluez", adapter_path,
+					"org.bluez.Adapter", "UnregisterAgent");
+	if (!msg) {
+		fprintf(stderr, "Can't allocate new method call\n");
+		return -1;
+	}
+
+	dbus_message_append_args(msg, DBUS_TYPE_OBJECT_PATH, &agent_path,
+							DBUS_TYPE_INVALID);
+
+	dbus_error_init(&err);
+
+	reply = dbus_connection_send_with_reply_and_block(conn, msg, -1, &err);
+
+	dbus_message_unref(msg);
+
+	if (!reply) {
+		fprintf(stderr, "Can't unregister agent\n");
+		if (dbus_error_is_set(&err)) {
+			fprintf(stderr, "%s\n", err.message);
+			dbus_error_free(&err);
+		}
+		return -1;
+	}
+
+	dbus_message_unref(reply);
+
+	dbus_connection_flush(conn);
+
+	dbus_connection_unregister_object_path(conn, agent_path);
+
+	return 0;
+}
+
+static int create_paired_device(DBusConnection *conn, const char *adapter_path,
+						const char *agent_path,
+						const char *capabilities,
+						const char *device)
+{
+	dbus_bool_t success;
+	DBusMessage *msg;
+
+	msg = dbus_message_new_method_call("org.bluez", adapter_path,
+						"org.bluez.Adapter",
+						"CreatePairedDevice");
+	if (!msg) {
+		fprintf(stderr, "Can't allocate new method call\n");
+		return -1;
+	}
+
+	dbus_message_append_args(msg, DBUS_TYPE_STRING, &device,
+					DBUS_TYPE_OBJECT_PATH, &agent_path,
+					DBUS_TYPE_STRING, &capabilities,
+					DBUS_TYPE_INVALID);
+
+	success = dbus_connection_send(conn, msg, NULL);
+
+	dbus_message_unref(msg);
+
+	if (!success) {
+		fprintf(stderr, "Not enough memory for message send\n");
+		return -1;
+	}
+
+	dbus_connection_flush(conn);
+
+	return 0;
+}
+
+static char *get_default_adapter_path(DBusConnection *conn)
+{
+	DBusMessage *msg, *reply;
+	DBusError err;
+	const char *reply_path;
+	char *path;
+
+	msg = dbus_message_new_method_call("org.bluez", "/",
+					"org.bluez.Manager", "DefaultAdapter");
+
+	if (!msg) {
+		fprintf(stderr, "Can't allocate new method call\n");
+		return NULL;
+	}
+
+	dbus_error_init(&err);
+
+	reply = dbus_connection_send_with_reply_and_block(conn, msg, -1, &err);
+
+	dbus_message_unref(msg);
+
+	if (!reply) {
+		fprintf(stderr,
+			"Can't get default adapter\n");
+		if (dbus_error_is_set(&err)) {
+			fprintf(stderr, "%s\n", err.message);
+			dbus_error_free(&err);
+		}
+		return NULL;
+	}
+
+	if (!dbus_message_get_args(reply, &err,
+					DBUS_TYPE_OBJECT_PATH, &reply_path,
+					DBUS_TYPE_INVALID)) {
+		fprintf(stderr,
+			"Can't get reply arguments\n");
+		if (dbus_error_is_set(&err)) {
+			fprintf(stderr, "%s\n", err.message);
+			dbus_error_free(&err);
+		}
+		return NULL;
+	}
+
+	path = strdup(reply_path);
+
+	dbus_message_unref(reply);
+
+	dbus_connection_flush(conn);
+
+	return path;
+}
+
+static char *get_adapter_path(DBusConnection *conn, const char *adapter)
+{
+	DBusMessage *msg, *reply;
+	DBusError err;
+	const char *reply_path;
+	char *path;
+
+	if (!adapter)
+		return get_default_adapter_path(conn);
+
+	msg = dbus_message_new_method_call("org.bluez", "/",
+					"org.bluez.Manager", "FindAdapter");
+
+	if (!msg) {
+		fprintf(stderr, "Can't allocate new method call\n");
+		return NULL;
+	}
+
+	dbus_message_append_args(msg, DBUS_TYPE_STRING, &adapter,
+					DBUS_TYPE_INVALID);
+
+	dbus_error_init(&err);
+
+	reply = dbus_connection_send_with_reply_and_block(conn, msg, -1, &err);
+
+	dbus_message_unref(msg);
+
+	if (!reply) {
+		fprintf(stderr,
+			"Can't find adapter %s\n", adapter);
+		if (dbus_error_is_set(&err)) {
+			fprintf(stderr, "%s\n", err.message);
+			dbus_error_free(&err);
+		}
+		return NULL;
+	}
+
+	if (!dbus_message_get_args(reply, &err,
+					DBUS_TYPE_OBJECT_PATH, &reply_path,
+					DBUS_TYPE_INVALID)) {
+		fprintf(stderr,
+			"Can't get reply arguments\n");
+		if (dbus_error_is_set(&err)) {
+			fprintf(stderr, "%s\n", err.message);
+			dbus_error_free(&err);
+		}
+		return NULL;
+	}
+
+	path = strdup(reply_path);
+
+	dbus_message_unref(reply);
+
+	dbus_connection_flush(conn);
+
+	return path;
+}
+
+static void usage(void)
+{
+	printf("Bluetooth agent ver %s\n\n", VERSION);
+
+	printf("Usage:\n"
+		"\tagent [--adapter adapter-path] [--path agent-path] <passkey> [<device>]\n"
+		"\n");
+}
+
+static struct option main_options[] = {
+	{ "adapter",	1, 0, 'a' },
+	{ "path",	1, 0, 'p' },
+	{ "capabilites",1, 0, 'c' },
+	{ "delay",	1, 0, 'd' },
+	{ "reject",	0, 0, 'r' },
+	{ "help",	0, 0, 'h' },
+	{ 0, 0, 0, 0 }
+};
+
+int main(int argc, char *argv[])
+{
+	const char *capabilities = "DisplayYesNo";
+	struct sigaction sa;
+	DBusConnection *conn;
+	char match_string[128], default_path[128], *adapter_id = NULL;
+	char *adapter_path = NULL, *agent_path = NULL, *device = NULL;
+	int opt;
+
+	snprintf(default_path, sizeof(default_path),
+					"/org/bluez/agent_%d", getpid());
+
+	while ((opt = getopt_long(argc, argv, "+a:p:c:d:rh", main_options, NULL)) != EOF) {
+		switch(opt) {
+		case 'a':
+			adapter_id = optarg;
+			break;
+		case 'p':
+			if (optarg[0] != '/') {
+				fprintf(stderr, "Invalid path\n");
+				exit(1);
+			}
+			agent_path = strdup(optarg);
+			break;
+		case 'c':
+			capabilities = optarg;
+			break;
+		case 'd':
+			passkey_delay = atoi(optarg);
+			break;
+		case 'r':
+			do_reject = 1;
+			break;
+		case 'h':
+			usage();
+			exit(0);
+		default:
+			exit(1);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+	optind = 0;
+
+	if (argc < 1) {
+		usage();
+		exit(1);
+	}
+
+	passkey_value = strdup(argv[0]);
+
+	if (argc > 1)
+		device = strdup(argv[1]);
+
+	if (!agent_path)
+		agent_path = strdup(default_path);
+
+	conn = dbus_bus_get(DBUS_BUS_SYSTEM, NULL);
+	if (!conn) {
+		fprintf(stderr, "Can't get on system bus");
+		exit(1);
+	}
+
+	adapter_path = get_adapter_path(conn, adapter_id);
+	if (!adapter_path)
+		exit(1);
+
+	if (!dbus_connection_register_object_path(conn, agent_path,
+							&agent_table, NULL)) {
+		fprintf(stderr, "Can't register object path for agent\n");
+		exit(1);
+	}
+
+	if (device) {
+		if (create_paired_device(conn, adapter_path, agent_path,
+						capabilities, device) < 0) {
+			dbus_connection_unref(conn);
+			exit(1);
+		}
+	} else {
+		if (register_agent(conn, adapter_path, agent_path,
+							capabilities) < 0) {
+			dbus_connection_unref(conn);
+			exit(1);
+		}
+	}
+
+	if (!dbus_connection_add_filter(conn, agent_filter, NULL, NULL))
+		fprintf(stderr, "Can't add signal filter");
+
+	snprintf(match_string, sizeof(match_string),
+			"interface=%s,member=NameOwnerChanged,arg0=%s",
+			DBUS_INTERFACE_DBUS, "org.bluez");
+
+	dbus_bus_add_match(conn, match_string, NULL);
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_flags   = SA_NOCLDSTOP;
+	sa.sa_handler = sig_term;
+	sigaction(SIGTERM, &sa, NULL);
+	sigaction(SIGINT,  &sa, NULL);
+
+	while (!__io_canceled && !__io_terminated) {
+		if (dbus_connection_read_write_dispatch(conn, 500) != TRUE)
+			break;
+	}
+
+	if (!__io_terminated && !device)
+		unregister_agent(conn, adapter_path, agent_path);
+
+	free(adapter_path);
+	free(agent_path);
+
+	free(passkey_value);
+
+	dbus_connection_unref(conn);
+
+	return 0;
+}
diff --git a/test/apitest b/test/apitest
new file mode 100755
index 0000000..b1c3f10
--- /dev/null
+++ b/test/apitest
@@ -0,0 +1,448 @@
+#!/usr/bin/env python
+
+import dbus
+import dbus.decorators
+import dbus.glib
+import gobject
+import sys
+import getopt
+from signal import *
+
+mgr_cmds = [ "InterfaceVersion", "ListAdapters", "DefaultAdapter" ]
+mgr_signals = [ "AdapterAdded", "AdapterRemoved" ]
+
+dev_cmds = [ "GetAddress",
+             "GetVersion",
+             "GetRevision",
+             "GetManufacturer",
+             "GetCompany",
+             "GetMode",
+             "SetMode",
+             "GetDiscoverableTimeout",
+             "SetDiscoverableTimeout",
+             "IsConnectable",
+             "IsDiscoverable",
+             "IsConnected",
+             "ListConnections",
+             "GetMajorClass",
+             "ListAvailableMinorClasses",
+             "GetMinorClass",
+             "SetMinorClass",
+             "GetServiceClasses",
+             "GetName",
+             "SetName",
+             "GetRemoteVersion",
+             "GetRemoteRevision",
+             "GetRemoteManufacturer",
+             "GetRemoteCompany",
+             "GetRemoteMajorClass",
+             "GetRemoteMinorClass",
+             "GetRemoteServiceClasses",
+             "GetRemoteClass",
+             "GetRemoteName",
+             "GetRemoteAlias",
+             "SetRemoteAlias",
+             "ClearRemoteAlias",
+             "LastSeen",
+             "LastUsed",
+             "DisconnectRemoteDevice",
+             "CreateBonding",
+             "CancelBondingProcess",
+             "RemoveBonding",
+             "HasBonding",
+             "ListBondings",
+             "GetPinCodeLength",
+             "GetEncryptionKeySize",
+             "DiscoverDevices",
+             "DiscoverDevicesWithoutNameResolving",
+             "CancelDiscovery",
+             "ListRemoteDevices",
+             "ListRecentRemoteDevices" ]
+dev_signals = [ "ModeChanged",
+                "NameChanged",
+                "MinorClassChanged",
+                "DiscoveryStarted",
+                "DiscoveryCompleted",
+                "RemoteDeviceFound",
+                "RemoteNameUpdated",
+                "RemoteNameFailed",
+                "RemoteAliasChanged"
+                "RemoteAliasCleared",
+                "RemoteDeviceConnected",
+                "RemoteDeviceDisconnectRequested",
+                "RemoteDeviceDisconnected",
+                "BondingCreated",
+                "BondingRemoved" ]
+
+dev_signals_filter = [ "/org/bluez/hci0", "/org/bluez/hci1",
+                       "/org/bluez/hci2", "/org/bluez/hci3",
+                       "/org/bluez/hci4", "/org/bluez/hci5",
+                       "/org/bluez/hci6", "/org/bluez/hci7" ]
+
+class Tester:
+    exit_events = []
+    dev_path = None
+    need_dev = False
+    listen = False
+    at_interrupt = None
+
+    def __init__(self, argv):
+        self.name = argv[0]
+
+        self.parse_args(argv[1:])
+
+        try:
+            self.dbus_setup()
+        except dbus.DBusException, e:
+            print 'Failed to do D-Bus setup: %s' % e
+            sys.exit(1)
+
+    def parse_args(self, argv):
+        try:
+            opts, args = getopt.getopt(argv, "hli:")
+        except getopt.GetoptError:
+            self.usage()
+            sys.exit(1)
+
+        for o, a in opts:
+            if o == "-h":
+                self.usage()
+                sys.exit()
+            elif o == "-l":
+                self.listen = True
+            elif o == "-i":
+                if a[0] == '/':
+                    self.dev_path = a
+                else:
+                    self.dev_path = '/org/bluez/%s' % a
+
+        if not (args or self.listen):
+            self.usage()
+            sys.exit(1)
+
+        if args:
+            self.cmd = args[0]
+            self.cmd_args = args[1:]
+
+    def dbus_dev_setup(self):
+        if not self.dev_path:
+            try:
+                self.dbus_mgr_setup()
+                self.dev_path = self.manager.DefaultAdapter()
+            except dbus.DBusException, e:
+                print 'Failed to get default device: %s' % e
+                sys.exit(1)
+        try:
+            obj = self.bus.get_object('org.bluez', self.dev_path)
+            self.device = dbus.Interface(obj, 'org.bluez.Adapter')
+        except dbus.DBusException, e:
+            print 'Failed to setup device path: %s' % e
+            sys.exit(1)
+
+    def dbus_dev_sig_setup(self):
+        try:
+           for signal in dev_signals:
+                for path in dev_signals_filter:
+                    self.bus.add_signal_receiver(self.dev_signal_handler,
+                                             signal, 'org.bluez.Adapter',
+                                             'org.bluez', path,
+                                             message_keyword='dbus_message')
+        except dbus.DBusException, e:
+            print 'Failed to setup signal handler for device path: %s' % e
+            sys.exit(1)
+
+    def dbus_mgr_sig_setup(self):
+        try:
+            for signal in mgr_signals:
+                self.bus.add_signal_receiver(self.mgr_signal_handler,
+                                         signal,'org.bluez.Manager',
+                                         'org.bluez', '/org/bluez')
+        except dbus.DBusException, e:
+            print 'Failed to setup signal handler for manager path: %s' % e
+            sys.exit(1)
+
+    def dbus_mgr_setup(self):
+        self.manager_obj = self.bus.get_object('org.bluez', '/org/bluez')
+        self.manager = dbus.Interface(self.manager_obj, 'org.bluez.Manager')
+
+    def dbus_setup(self):
+        self.bus = dbus.SystemBus()
+
+    def usage(self):
+        print 'Usage: %s [-i <dev>] [-l] [-h] <cmd> [arg1..]' % self.name
+        print '  -i <dev>   Specify device (e.g. "hci0" or "/org/bluez/hci0")'
+        print '  -l         Listen for events (no command required)'
+        print '  -h         Show this help'
+        print 'Manager commands:'
+        for cmd in mgr_cmds:
+            print '\t%s' % cmd
+        print 'Adapter commands:'
+        for cmd in dev_cmds:
+            print '\t%s' % cmd
+
+    #@dbus.decorators.explicitly_pass_message
+    def dev_signal_handler(*args, **keywords):
+        dbus_message = keywords["dbus_message"]
+        print '%s - %s: ' % (dbus_message.get_member(), dbus_message.get_path()),
+        for arg in args[1:]:
+            print '%s   ' % arg,
+        print
+
+    #@dbus.decorators.explicitly_pass_message
+    def mgr_signal_handler(*args, **keywords):
+        dbus_message = keywords["dbus_message"]
+        print '%s: ' % dbus_message.get_member()
+        for arg in args[1:]:
+            print '%s   ' % arg,
+        print
+
+    def signal_cb(self, sig, frame):
+        print 'Caught signal, exiting'
+        if self.at_interrupt:
+            self.at_interrupt()
+        self.main_loop.quit()
+
+    def call_mgr_dbus_func(self):
+        if self.cmd == 'InterfaceVersion':
+            try:
+                print self.manager.InterfaceVersion()
+            except dbus.DBusException, e:
+                print 'Sending %s failed: %s' % (self.cmd, e)
+        if self.cmd == 'ListAdapters':
+            try:
+                devices = self.manager.ListAdapters()
+            except dbus.DBusException, e:
+                print 'Sending %s failed: %s' % (self.cmd, e)
+                sys.exit(1)
+            for device in devices:
+                print device
+        elif self.cmd == 'DefaultAdapter':
+            try:
+                print self.manager.DefaultAdapter()
+            except dbus.DBusException, e:
+                print 'Sending %s failed: %s' % (self.cmd, e)
+                sys.exit(1)
+
+    def call_dev_dbus_func(self):
+       try:
+           if self.cmd == 'GetAddress':
+               print self.device.GetAddress()
+           elif self.cmd == 'GetManufacturer':
+               print self.device.GetManufacturer()
+           elif self.cmd == 'GetVersion':
+               print self.device.GetVersion()
+           elif self.cmd == 'GetRevision':
+               print self.device.GetRevision()
+           elif self.cmd == 'GetCompany':
+               print self.device.GetCompany()
+           elif self.cmd == 'GetMode':
+               print self.device.GetMode()
+           elif self.cmd == 'SetMode':
+               if len(self.cmd_args) == 1:
+                   self.device.SetMode(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> SetMode scan_mode' % self.name
+           elif self.cmd == 'GetDiscoverableTimeout':
+               print '%u' % (self.device.GetDiscoverableTimeout())
+           elif self.cmd == 'SetDiscoverableTimeout':
+               if len(self.cmd_args) == 1:
+                   self.device.SetDiscoverableTimeout(dbus.UInt32(self.cmd_args[0]))
+               else:
+                   print 'Usage: %s -i <dev> SetDiscoverableTimeout timeout' % self.name
+           elif self.cmd == 'IsConnectable':
+               print self.device.IsConnectable()
+           elif self.cmd == 'IsDiscoverable':
+               print self.device.IsDiscoverable()
+           elif self.cmd == 'IsConnected':
+               if len(self.cmd_args) == 1:
+                   print self.device.IsConnected(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> IsConnected address' % self.name
+           elif self.cmd == 'ListConnections':
+               print self.device.ListConnections()
+           elif self.cmd == 'GetMajorClass':
+               print self.device.GetMajorClass()
+           elif self.cmd == 'ListAvailableMinorClasses':
+               print self.device.ListAvailableMinorClasses()
+           elif self.cmd == 'GetMinorClass':
+               print self.device.GetMinorClass()
+           elif self.cmd == 'SetMinorClass':
+               if len(self.cmd_args) == 1:
+                   self.device.SetMinorClass(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> SetMinorClass minor' % self.name
+           elif self.cmd == 'GetServiceClasses':
+               classes = self.device.GetServiceClasses()
+               for clas in classes: 
+                   print clas,
+           elif self.cmd == 'GetName':
+               print self.device.GetName()
+           elif self.cmd == 'SetName':
+               if len(self.cmd_args) == 1:
+                   self.device.SetName(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> SetName newname' % self.name
+           elif self.cmd == 'GetRemoteName':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteName(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteName address' % self.name
+           elif self.cmd == 'GetRemoteVersion':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteVersion(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteVersion address' % self.name
+           elif self.cmd == 'GetRemoteRevision':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteRevision(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteRevision address' % self.name
+           elif self.cmd == 'GetRemoteManufacturer':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteManufacturer(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteManufacturer address' % self.name
+           elif self.cmd == 'GetRemoteCompany':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteCompany(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteCompany address' % self.name
+           elif self.cmd == 'GetRemoteAlias':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteAlias(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteAlias address' % self.name
+           elif self.cmd == 'GetRemoteMajorClass':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteMajorClass(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteMajorClass address' % self.name
+           elif self.cmd == 'GetRemoteMinorClass':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteMinorClass(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteMinorClass address' % self.name
+           elif self.cmd == 'GetRemoteServiceClasses':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetRemoteServiceClasses(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetRemoteServiceClasses address' % self.name
+           elif self.cmd == 'SetRemoteAlias':
+               if len(self.cmd_args) == 2:
+                   self.device.SetRemoteAlias(self.cmd_args[0], self.cmd_args[1])
+               else:
+                   print 'Usage: %s -i <dev> SetRemoteAlias address alias' % self.name
+           elif self.cmd == 'ClearRemoteAlias':
+               if len(self.cmd_args) == 1:
+                   print self.device.ClearRemoteAlias(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> ClearRemoteAlias address' % self.name
+           elif self.cmd == 'LastSeen':
+               if len(self.cmd_args) == 1:
+                   print self.device.LastSeen(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> LastSeen address' % self.name
+           elif self.cmd == 'LastUsed':
+               if len(self.cmd_args) == 1:
+                   print self.device.LastUsed(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> LastUsed address' % self.name
+           elif self.cmd == 'DisconnectRemoteDevice':
+               if len(self.cmd_args) == 1:
+                   print self.device.LastUsed(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> DisconnectRemoteDevice address' % self.name
+           elif self.cmd == 'CreateBonding':
+               if len(self.cmd_args) == 1:
+                   print self.device.CreateBonding(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> CreateBonding address' % self.name
+           elif self.cmd == 'RemoveBonding':
+               if len(self.cmd_args) == 1:
+                   print self.device.RemoveBonding(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> RemoveBonding address' % self.name
+           elif self.cmd == 'CancelBondingProcess':
+               if len(self.cmd_args) == 1:
+                   print self.device.CancelBondingProcess(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> CancelBondingProcess address' % self.name
+           elif self.cmd == 'HasBonding':
+               if len(self.cmd_args) == 1:
+                   print self.device.HasBonding(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> HasBonding address' % self.name
+           elif self.cmd == 'ListBondings':
+               bondings = self.device.ListBondings()
+               for bond in bondings: 
+                   print bond,
+           elif self.cmd == 'GetPinCodeLength':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetPinCodeLength(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetPinCodeLength address' % self.name
+           elif self.cmd == 'GetEncryptionKeySize':
+               if len(self.cmd_args) == 1:
+                   print self.device.GetEncryptionKeySize(self.cmd_args[0])
+               else:
+                   print 'Usage: %s -i <dev> GetEncryptionKeySize address' % self.name
+           elif self.cmd == 'DiscoverDevices':
+               print self.device.DiscoverDevices()
+           elif self.cmd == 'DiscoverDevicesWithoutNameResolving':
+               print self.device.DiscoverDevicesWithoutNameResolving()
+           elif self.cmd == 'ListRemoteDevices':
+               devices = self.device.ListRemoteDevices()
+               for device in devices: 
+                   print device,
+           elif self.cmd == 'ListRecentRemoteDevices':
+               if len(self.cmd_args) == 1:
+                   devices = self.device.ListRecentRemoteDevices(self.cmd_args[0])
+                   for device in devices: 
+                       print device,
+               else:
+                   print 'Usage: %s -i <dev> ListRecentRemoteDevices date' % self.name
+           else:
+                # FIXME: remove at future version
+                print 'Script Error: Method %s not found. Maybe a mispelled word.' % (self.cmd_args)
+       except dbus.DBusException, e:
+           print '%s failed: %s' % (self.cmd, e)
+           sys.exit(1)
+
+    def run(self):
+        # Manager methods
+        if self.listen:
+            self.dbus_mgr_sig_setup()
+            self.dbus_dev_sig_setup()
+            print 'Listening for events...'
+
+        if self.cmd in mgr_cmds:
+            try:
+                self.dbus_mgr_setup()
+            except dbus.DBusException, e:
+                print 'Failed to setup manager interface: %s' % e
+                sys.exit(1)
+            self.call_mgr_dbus_func()
+        elif self.cmd in dev_cmds:
+            try:
+                self.dbus_dev_setup()
+            except dbus.DBusException, e:
+                print 'Failed to setup device interface: %s' % e
+                sys.exit(1)
+            self.call_dev_dbus_func()
+        elif not self.listen:
+            print 'Unknown command: %s' % self.cmd
+            self.usage()
+            sys.exit(1)
+
+        if self.listen:
+            signal(SIGINT, self.signal_cb)
+            signal(SIGTERM, self.signal_cb)
+            self.main_loop = gobject.MainLoop()
+            self.main_loop.run()
+
+if __name__ == '__main__':
+    gobject.threads_init()
+    dbus.glib.init_threads()
+
+    tester = Tester(sys.argv)
+    tester.run()
diff --git a/test/attest.c b/test/attest.c
new file mode 100644
index 0000000..12ba682
--- /dev/null
+++ b/test/attest.c
@@ -0,0 +1,183 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2001-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/rfcomm.h>
+
+static int at_command(int fd, char *cmd, int to)
+{
+	fd_set rfds;
+	struct timeval timeout;
+	char buf[1024];
+	int sel, len, i, n;
+
+	len = write(fd, cmd, strlen(cmd));
+
+	for (i = 0; i < 100; i++) {
+
+		FD_ZERO(&rfds);
+		FD_SET(fd, &rfds);
+
+		timeout.tv_sec = 0;
+		timeout.tv_usec = to;
+
+		if ((sel = select(fd + 1, &rfds, NULL, NULL, &timeout)) > 0) {
+
+			if (FD_ISSET(fd, &rfds)) {
+				memset(buf, 0, sizeof(buf));
+				len = read(fd, buf, sizeof(buf));
+				for (n = 0; n < len; n++)
+					printf("%c", buf[n]);
+				if (strstr(buf, "\r\nOK") != NULL)
+					break;
+				if (strstr(buf, "\r\nERROR") != NULL)
+					break;
+				if (strstr(buf, "\r\nCONNECT") != NULL)
+					break;
+			}
+
+		}
+
+	}
+
+	return 0;
+}
+
+static int open_device(char *device)
+{
+	struct termios ti;
+	int fd;
+
+	fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	if (fd < 0) {
+		fprintf(stderr, "Can't open serial port: %s (%d)\n",
+							strerror(errno), errno);
+		return -1;
+	}
+
+	tcflush(fd, TCIOFLUSH);
+
+	/* Switch tty to RAW mode */
+	cfmakeraw(&ti);
+	tcsetattr(fd, TCSANOW, &ti);
+
+	return fd;
+}
+
+static int open_socket(bdaddr_t *bdaddr, uint8_t channel)
+{
+	struct sockaddr_rc addr;
+	int sk;
+
+	sk = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+	if (sk < 0) {
+		fprintf(stderr, "Can't create socket: %s (%d)\n",
+							strerror(errno), errno);
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.rc_family = AF_BLUETOOTH;
+	bacpy(&addr.rc_bdaddr, BDADDR_ANY);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		fprintf(stderr, "Can't bind socket: %s (%d)\n",
+							strerror(errno), errno);
+		close(sk);
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.rc_family = AF_BLUETOOTH;
+	bacpy(&addr.rc_bdaddr, bdaddr);
+	addr.rc_channel = channel;
+
+	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		fprintf(stderr, "Can't connect: %s (%d)\n",
+							strerror(errno), errno);
+		close(sk);
+		return -1;
+	}
+
+	return sk;
+}
+
+static void usage(void)
+{
+	printf("Usage:\n\tattest <device> | <bdaddr> [channel]\n");
+}
+
+int main(int argc, char *argv[])
+{
+	int fd;
+
+	bdaddr_t bdaddr;
+	uint8_t channel;
+
+	switch (argc) {
+	case 2:
+		str2ba(argv[1], &bdaddr);
+		channel = 1;
+		break;
+	case 3:
+		str2ba(argv[1], &bdaddr);
+		channel = atoi(argv[2]);
+		break;
+	default:
+		usage();
+		exit(-1);
+	}
+
+	if (bacmp(BDADDR_ANY, &bdaddr)) {
+		printf("Connecting to %s on channel %d\n", argv[1], channel);
+		fd = open_socket(&bdaddr, channel);
+	} else {
+		printf("Opening device %s\n", argv[1]);
+		fd = open_device(argv[1]);
+	}
+
+	if (fd < 0)
+		exit(-2);
+
+	at_command(fd, "ATZ\r\n", 10000);
+	at_command(fd, "AT+CPBS=\"ME\"\r\n", 10000);
+	at_command(fd, "AT+CPBR=1,100\r\n", 100000);
+
+	close(fd);
+
+	return 0;
+}
diff --git a/test/avtest.c b/test/avtest.c
new file mode 100644
index 0000000..168326f
--- /dev/null
+++ b/test/avtest.c
@@ -0,0 +1,869 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2007-2010  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2009-2010  Nokia Corporation
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/l2cap.h>
+#include <bluetooth/sdp.h>
+
+#define AVDTP_PKT_TYPE_SINGLE		0x00
+#define AVDTP_PKT_TYPE_START		0x01
+#define AVDTP_PKT_TYPE_CONTINUE		0x02
+#define AVDTP_PKT_TYPE_END		0x03
+
+#define AVDTP_MSG_TYPE_COMMAND		0x00
+#define AVDTP_MSG_TYPE_GEN_REJECT	0x01
+#define AVDTP_MSG_TYPE_ACCEPT		0x02
+#define AVDTP_MSG_TYPE_REJECT		0x03
+
+#define AVDTP_DISCOVER			0x01
+#define AVDTP_GET_CAPABILITIES		0x02
+#define AVDTP_SET_CONFIGURATION		0x03
+#define AVDTP_GET_CONFIGURATION		0x04
+#define AVDTP_RECONFIGURE		0x05
+#define AVDTP_OPEN			0x06
+#define AVDTP_START			0x07
+#define AVDTP_CLOSE			0x08
+#define AVDTP_SUSPEND			0x09
+#define AVDTP_ABORT			0x0A
+
+#define AVDTP_SEP_TYPE_SOURCE		0x00
+#define AVDTP_SEP_TYPE_SINK		0x01
+
+#define AVDTP_MEDIA_TYPE_AUDIO		0x00
+#define AVDTP_MEDIA_TYPE_VIDEO		0x01
+#define AVDTP_MEDIA_TYPE_MULTIMEDIA	0x02
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+
+struct avdtp_header {
+	uint8_t message_type:2;
+	uint8_t packet_type:2;
+	uint8_t transaction:4;
+	uint8_t signal_id:6;
+	uint8_t rfa0:2;
+} __attribute__ ((packed));
+
+struct seid_info {
+	uint8_t rfa0:1;
+	uint8_t inuse:1;
+	uint8_t seid:6;
+	uint8_t rfa2:3;
+	uint8_t type:1;
+	uint8_t media_type:4;
+} __attribute__ ((packed));
+
+struct avdtp_start_header {
+	uint8_t message_type:2;
+	uint8_t packet_type:2;
+	uint8_t transaction:4;
+	uint8_t no_of_packets;
+	uint8_t signal_id:6;
+	uint8_t rfa0:2;
+} __attribute__ ((packed));
+
+struct avdtp_continue_header {
+	uint8_t message_type:2;
+	uint8_t packet_type:2;
+	uint8_t transaction:4;
+} __attribute__ ((packed));
+
+struct avctp_header {
+	uint8_t ipid:1;
+	uint8_t cr:1;
+	uint8_t packet_type:2;
+	uint8_t transaction:4;
+	uint16_t pid;
+} __attribute__ ((packed));
+#define AVCTP_HEADER_LENGTH 3
+
+#elif __BYTE_ORDER == __BIG_ENDIAN
+
+struct avdtp_header {
+	uint8_t transaction:4;
+	uint8_t packet_type:2;
+	uint8_t message_type:2;
+	uint8_t rfa0:2;
+	uint8_t signal_id:6;
+} __attribute__ ((packed));
+
+struct seid_info {
+	uint8_t seid:6;
+	uint8_t inuse:1;
+	uint8_t rfa0:1;
+	uint8_t media_type:4;
+	uint8_t type:1;
+	uint8_t rfa2:3;
+} __attribute__ ((packed));
+
+struct avdtp_start_header {
+	uint8_t transaction:4;
+	uint8_t packet_type:2;
+	uint8_t message_type:2;
+	uint8_t no_of_packets;
+	uint8_t rfa0:2;
+	uint8_t signal_id:6;
+} __attribute__ ((packed));
+
+struct avdtp_continue_header {
+	uint8_t transaction:4;
+	uint8_t packet_type:2;
+	uint8_t message_type:2;
+} __attribute__ ((packed));
+
+struct avctp_header {
+	uint8_t transaction:4;
+	uint8_t packet_type:2;
+	uint8_t cr:1;
+	uint8_t ipid:1;
+	uint16_t pid;
+} __attribute__ ((packed));
+#define AVCTP_HEADER_LENGTH 3
+
+#else
+#error "Unknown byte order"
+#endif
+
+#define AVCTP_COMMAND		0
+#define AVCTP_RESPONSE		1
+
+#define AVCTP_PACKET_SINGLE	0
+
+static const unsigned char media_transport[] = {
+		0x01,	/* Media transport category */
+		0x00,
+		0x07,	/* Media codec category */
+		0x06,
+		0x00,	/* Media type audio */
+		0x00,	/* Codec SBC */
+		0x22,	/* 44.1 kHz, stereo */
+		0x15,	/* 16 blocks, 8 subbands */
+		0x02,
+		0x33,
+};
+
+static int media_sock = -1;
+
+static void dump_avctp_header(struct avctp_header *hdr)
+{
+	printf("TL %d PT %d CR %d IPID %d PID 0x%04x\n", hdr->transaction,
+			hdr->packet_type, hdr->cr, hdr->ipid, ntohs(hdr->pid));
+}
+
+static void dump_avdtp_header(struct avdtp_header *hdr)
+{
+	printf("TL %d PT %d MT %d SI %d\n", hdr->transaction,
+			hdr->packet_type, hdr->message_type, hdr->signal_id);
+}
+
+static void dump_buffer(const unsigned char *buf, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		printf("%02x ", buf[i]);
+	printf("\n");
+}
+
+static void process_avdtp(int srv_sk, int sk, unsigned char reject,
+								int fragment)
+{
+	unsigned char buf[672];
+	ssize_t len;
+
+	while (1) {
+		struct avdtp_header *hdr = (void *) buf;
+
+		len = read(sk, buf, sizeof(buf));
+		if (len <= 0) {
+			perror("Read failed");
+			break;
+		}
+
+		dump_buffer(buf, len);
+		dump_avdtp_header(hdr);
+
+		if (hdr->packet_type != AVDTP_PKT_TYPE_SINGLE) {
+			fprintf(stderr, "Only single packets are supported\n");
+			break;
+		}
+
+		if (hdr->message_type != AVDTP_MSG_TYPE_COMMAND) {
+			fprintf(stderr, "Ignoring non-command messages\n");
+			continue;
+		}
+
+		switch (hdr->signal_id) {
+		case AVDTP_DISCOVER:
+			if (reject == AVDTP_DISCOVER) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[2] = 0x29; /* Unsupported configuration */
+				printf("Rejecting discover command\n");
+				len = write(sk, buf, 3);
+			} else {
+				struct seid_info *sei = (void *) (buf + 2);
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				buf[2] = 0x00;
+				buf[3] = 0x00;
+				sei->seid = 0x01;
+				sei->type = AVDTP_SEP_TYPE_SINK;
+				sei->media_type = AVDTP_MEDIA_TYPE_AUDIO;
+				printf("Accepting discover command\n");
+				len = write(sk, buf, 4);
+			}
+			break;
+
+		case AVDTP_GET_CAPABILITIES:
+			if (reject == AVDTP_GET_CAPABILITIES) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[2] = 0x29; /* Unsupported configuration */
+				printf("Rejecting get capabilties command\n");
+				len = write(sk, buf, 3);
+			} else if (fragment) {
+				struct avdtp_start_header *start = (void *) buf;
+
+				printf("Sending fragmented reply to getcap\n");
+
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+
+				/* Start packet */
+				hdr->packet_type = AVDTP_PKT_TYPE_START;
+				start->signal_id = AVDTP_GET_CAPABILITIES;
+				start->no_of_packets = 3;
+				memcpy(&buf[3], media_transport,
+						sizeof(media_transport));
+				len = write(sk, buf,
+						3 + sizeof(media_transport));
+
+				/* Continue packet */
+				hdr->packet_type = AVDTP_PKT_TYPE_CONTINUE;
+				memcpy(&buf[1], media_transport,
+						sizeof(media_transport));
+				len = write(sk, buf,
+						1 + sizeof(media_transport));
+
+				/* End packet */
+				hdr->packet_type = AVDTP_PKT_TYPE_END;
+				memcpy(&buf[1], media_transport,
+						sizeof(media_transport));
+				len = write(sk, buf,
+						1 + sizeof(media_transport));
+			} else {
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				memcpy(&buf[2], media_transport,
+						sizeof(media_transport));
+				printf("Accepting get capabilities command\n");
+				len = write(sk, buf,
+						2 + sizeof(media_transport));
+			}
+			break;
+
+		case AVDTP_SET_CONFIGURATION:
+			if (reject == AVDTP_SET_CONFIGURATION) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[2] = buf[4];
+				buf[3] = 0x13; /* SEP In Use */
+				printf("Rejecting set configuration command\n");
+				len = write(sk, buf, 4);
+			} else {
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				printf("Accepting set configuration command\n");
+				len = write(sk, buf, 2);
+			}
+			break;
+
+		case AVDTP_GET_CONFIGURATION:
+			if (reject == AVDTP_GET_CONFIGURATION) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[2] = 0x12; /* Bad ACP SEID */
+				printf("Rejecting get configuration command\n");
+				len = write(sk, buf, 3);
+			} else {
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				printf("Accepting get configuration command\n");
+				len = write(sk, buf, 2);
+			}
+			break;
+
+		case AVDTP_OPEN:
+			if (reject == AVDTP_OPEN) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[2] = 0x31; /* Bad State */
+				printf("Rejecting open command\n");
+				len = write(sk, buf, 3);
+			} else {
+				struct sockaddr_l2 addr;
+				socklen_t optlen;
+
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				printf("Accepting open command\n");
+				len = write(sk, buf, 2);
+
+				memset(&addr, 0, sizeof(addr));
+				optlen = sizeof(addr);
+
+				media_sock = accept(srv_sk,
+						(struct sockaddr *) &addr,
+								&optlen);
+				if (media_sock < 0) {
+					perror("Accept failed");
+					break;
+				}
+			}
+			break;
+
+		case AVDTP_START:
+			if (reject == AVDTP_ABORT)
+				printf("Ignoring start to cause abort");
+			else if (reject == AVDTP_START) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[3] = 0x31; /* Bad State */
+				printf("Rejecting start command\n");
+				len = write(sk, buf, 4);
+			} else {
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				printf("Accepting start command\n");
+				len = write(sk, buf, 2);
+			}
+			break;
+
+		case AVDTP_CLOSE:
+			if (reject == AVDTP_CLOSE) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[2] = 0x31; /* Bad State */
+				printf("Rejecting close command\n");
+				len = write(sk, buf, 3);
+			} else {
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				printf("Accepting close command\n");
+				len = write(sk, buf, 2);
+				if (media_sock >= 0) {
+					close(media_sock);
+					media_sock = -1;
+				}
+			}
+			break;
+
+		case AVDTP_SUSPEND:
+			if (reject == AVDTP_SUSPEND) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[3] = 0x31; /* Bad State */
+				printf("Rejecting suspend command\n");
+				len = write(sk, buf, 4);
+			} else {
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				printf("Accepting suspend command\n");
+				len = write(sk, buf, 2);
+			}
+			break;
+
+		case AVDTP_ABORT:
+			hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+			printf("Accepting abort command\n");
+			len = write(sk, buf, 2);
+			if (media_sock >= 0) {
+				close(media_sock);
+				media_sock = -1;
+			}
+			break;
+
+		default:
+			buf[1] = 0x00;
+			printf("Unknown command\n");
+			len = write(sk, buf, 2);
+			break;
+		}
+	}
+}
+
+static void process_avctp(int sk, int reject)
+{
+	unsigned char buf[672];
+	ssize_t len;
+
+	while (1) {
+		struct avctp_header *hdr = (void *) buf;
+
+		len = read(sk, buf, sizeof(buf));
+		if (len <= 0) {
+			perror("Read failed");
+			break;
+		}
+
+		dump_buffer(buf, len);
+
+		if (len >= AVCTP_HEADER_LENGTH)
+			dump_avctp_header(hdr);
+	}
+}
+
+static int set_minimum_mtu(int sk)
+{
+	struct l2cap_options l2o;
+	socklen_t optlen;
+
+	memset(&l2o, 0, sizeof(l2o));
+	optlen = sizeof(l2o);
+
+	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, &optlen) < 0) {
+		perror("getsockopt");
+		return -1;
+	}
+
+	l2o.imtu = 48;
+	l2o.omtu = 48;
+
+	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, sizeof(l2o)) < 0) {
+		perror("setsockopt");
+		return -1;
+	}
+
+	return 0;
+}
+
+static void do_listen(const bdaddr_t *src, unsigned char reject, int fragment)
+{
+	struct sockaddr_l2 addr;
+	socklen_t optlen;
+	int sk, nsk;
+
+	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sk < 0) {
+		perror("Can't create socket");
+		return;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, src);
+	addr.l2_psm = htobs(25);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		perror("Can't bind socket");
+		goto error;
+	}
+
+	if (fragment)
+		set_minimum_mtu(sk);
+
+	if (listen(sk, 10)) {
+		perror("Can't listen on the socket");
+		goto error;
+	}
+
+	while (1) {
+		memset(&addr, 0, sizeof(addr));
+		optlen = sizeof(addr);
+
+		nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
+		if (nsk < 0) {
+			perror("Accept failed");
+			continue;
+		}
+
+		process_avdtp(sk, nsk, reject, fragment);
+
+		if (media_sock >= 0) {
+			close(media_sock);
+			media_sock = -1;
+		}
+
+		close(nsk);
+	}
+
+error:
+	close(sk);
+}
+
+static int do_connect(const bdaddr_t *src, const bdaddr_t *dst, int avctp,
+								int fragment)
+{
+	struct sockaddr_l2 addr;
+	int sk, err;
+
+	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sk < 0) {
+		perror("Can't create socket");
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, src);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		perror("Can't bind socket");
+		goto error;
+	}
+
+	if (fragment)
+		set_minimum_mtu(sk);
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, dst);
+	addr.l2_psm = htobs(avctp ? 23 : 25);
+
+	err = connect(sk, (struct sockaddr *) &addr, sizeof(addr));
+	if (err < 0) {
+		perror("Unable to connect");
+		goto error;
+	}
+
+	return sk;
+
+error:
+	close(sk);
+	return -1;
+}
+
+static void do_avdtp_send(int sk, const bdaddr_t *src, const bdaddr_t *dst,
+				unsigned char cmd, int invalid, int preconf)
+{
+	unsigned char buf[672];
+	struct avdtp_header *hdr = (void *) buf;
+	ssize_t len;
+
+	memset(buf, 0, sizeof(buf));
+
+	switch (cmd) {
+	case AVDTP_DISCOVER:
+		if (invalid)
+			hdr->message_type = 0x01;
+		else
+			hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_DISCOVER;
+		len = write(sk, buf, 2);
+		break;
+
+	case AVDTP_GET_CAPABILITIES:
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_GET_CAPABILITIES;
+		buf[2] = 1 << 2; /* SEID 1 */
+		len = write(sk, buf, invalid ? 2 : 3);
+		break;
+
+	case AVDTP_SET_CONFIGURATION:
+		if (preconf)
+			do_avdtp_send(sk, src, dst, cmd, 0, 0);
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_SET_CONFIGURATION;
+		buf[2] = 1 << 2; /* ACP SEID */
+		buf[3] = 1 << 2; /* INT SEID */
+		memcpy(&buf[4], media_transport, sizeof(media_transport));
+		if (invalid)
+			buf[5] = 0x01; /* LOSC != 0 */
+		len = write(sk, buf, 4 + sizeof(media_transport));
+		break;
+
+	case AVDTP_GET_CONFIGURATION:
+		if (preconf)
+			do_avdtp_send(sk, src, dst, AVDTP_SET_CONFIGURATION, 0, 0);
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_GET_CONFIGURATION;
+		if (invalid)
+			buf[2] = 13 << 2; /* Invalid ACP SEID */
+		else
+			buf[2] = 1 << 2; /* Valid ACP SEID */
+		len = write(sk, buf, 3);
+		break;
+
+	case AVDTP_OPEN:
+		if (preconf)
+			do_avdtp_send(sk, src, dst, AVDTP_SET_CONFIGURATION, 0, 0);
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_OPEN;
+		buf[2] = 1 << 2; /* ACP SEID */
+		len = write(sk, buf, 3);
+		break;
+
+	case AVDTP_START:
+		if (preconf)
+			do_avdtp_send(sk, src, dst, AVDTP_SET_CONFIGURATION, 0, 0);
+		if (!invalid)
+			do_avdtp_send(sk, src, dst, AVDTP_OPEN, 0, 0);
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_START;
+		buf[2] = 1 << 2; /* ACP SEID */
+		len = write(sk, buf, 3);
+		break;
+
+	case AVDTP_CLOSE:
+		if (preconf) {
+			do_avdtp_send(sk, src, dst, AVDTP_SET_CONFIGURATION, 0, 0);
+			do_avdtp_send(sk, src, dst, AVDTP_OPEN, 0, 0);
+		}
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_CLOSE;
+		if (invalid)
+			buf[2] = 13 << 2; /* Invalid ACP SEID */
+		else
+			buf[2] = 1 << 2; /* Valid ACP SEID */
+		len = write(sk, buf, 3);
+		break;
+
+	case AVDTP_SUSPEND:
+		if (invalid)
+			do_avdtp_send(sk, src, dst, AVDTP_OPEN, 0, preconf);
+		else
+			do_avdtp_send(sk, src, dst, AVDTP_START, 0, preconf);
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_SUSPEND;
+		buf[2] = 1 << 2; /* ACP SEID */
+		len = write(sk, buf, 3);
+		break;
+
+	case AVDTP_ABORT:
+		do_avdtp_send(sk, src, dst, AVDTP_OPEN, 0, 1);
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = AVDTP_ABORT;
+		buf[2] = 1 << 2; /* ACP SEID */
+		len = write(sk, buf, 3);
+		break;
+
+	default:
+		hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
+		hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
+		hdr->signal_id = cmd;
+		len = write(sk, buf, 2);
+		break;
+	}
+
+	do {
+		len = read(sk, buf, sizeof(buf));
+
+		dump_buffer(buf, len);
+		dump_avdtp_header(hdr);
+	} while (len < 2 || (hdr->message_type != AVDTP_MSG_TYPE_ACCEPT &&
+				hdr->message_type != AVDTP_MSG_TYPE_REJECT &&
+				hdr->message_type != AVDTP_MSG_TYPE_GEN_REJECT));
+
+	if (cmd == AVDTP_OPEN && len >= 2 &&
+				hdr->message_type == AVDTP_MSG_TYPE_ACCEPT)
+		media_sock = do_connect(src, dst, 0, 0);
+}
+
+static void do_avctp_send(int sk, int invalid)
+{
+	unsigned char buf[672];
+	struct avctp_header *hdr = (void *) buf;
+	unsigned char play_pressed[] = { 0x00, 0x48, 0x7c, 0x44, 0x00 };
+	ssize_t len;
+
+	memset(buf, 0, sizeof(buf));
+
+	hdr->packet_type = AVCTP_PACKET_SINGLE;
+	hdr->cr = AVCTP_COMMAND;
+	if (invalid)
+		hdr->pid = 0xffff;
+	else
+		hdr->pid = htons(AV_REMOTE_SVCLASS_ID);
+
+	memcpy(&buf[AVCTP_HEADER_LENGTH], play_pressed, sizeof(play_pressed));
+
+	len = write(sk, buf, AVCTP_HEADER_LENGTH + sizeof(play_pressed));
+
+	len = read(sk, buf, sizeof(buf));
+
+	dump_buffer(buf, len);
+	if (len >= AVCTP_HEADER_LENGTH)
+		dump_avctp_header(hdr);
+}
+
+static void usage()
+{
+	printf("avtest - Audio/Video testing ver %s\n", VERSION);
+	printf("Usage:\n"
+		"\tavtest [options] [remote address]\n");
+	printf("Options:\n"
+		"\t--device <hcidev>    HCI device\n"
+		"\t--reject <command>   Reject command\n"
+		"\t--send <command>     Send command\n"
+		"\t--preconf            Configure stream before actual command\n"
+		"\t--wait <N>           Wait N seconds before exiting\n"
+		"\t--fragment           Use minimum MTU and fragmented messages\n"
+		"\t--invalid <command>  Send invalid command\n");
+}
+
+static struct option main_options[] = {
+	{ "help",	0, 0, 'h' },
+	{ "device",	1, 0, 'i' },
+	{ "reject",	1, 0, 'r' },
+	{ "send",	1, 0, 's' },
+	{ "invalid",	1, 0, 'f' },
+	{ "preconf",	0, 0, 'c' },
+	{ "fragment",   0, 0, 'F' },
+	{ "avctp",	0, 0, 'C' },
+	{ "wait",	1, 0, 'w' },
+	{ 0, 0, 0, 0 }
+};
+
+static unsigned char parse_cmd(const char *arg)
+{
+	if (!strncmp(arg, "discov", 6))
+		return AVDTP_DISCOVER;
+	else if (!strncmp(arg, "capa", 4))
+		return AVDTP_GET_CAPABILITIES;
+	else if (!strncmp(arg, "getcapa", 7))
+		return AVDTP_GET_CAPABILITIES;
+	else if (!strncmp(arg, "setconf", 7))
+		return AVDTP_SET_CONFIGURATION;
+	else if (!strncmp(arg, "getconf", 7))
+		return AVDTP_GET_CONFIGURATION;
+	else if (!strncmp(arg, "open", 4))
+		return AVDTP_OPEN;
+	else if (!strncmp(arg, "start", 5))
+		return AVDTP_START;
+	else if (!strncmp(arg, "close", 5))
+		return AVDTP_CLOSE;
+	else if (!strncmp(arg, "suspend", 7))
+		return AVDTP_SUSPEND;
+	else if (!strncmp(arg, "abort", 7))
+		return AVDTP_ABORT;
+	else
+		return atoi(arg);
+}
+
+enum {
+	MODE_NONE, MODE_REJECT, MODE_SEND,
+};
+
+int main(int argc, char *argv[])
+{
+	unsigned char cmd = 0x00;
+	bdaddr_t src, dst;
+	int opt, mode = MODE_NONE, sk, invalid = 0, preconf = 0, fragment = 0;
+	int avctp = 0, wait_before_exit = 0;
+
+	bacpy(&src, BDADDR_ANY);
+	bacpy(&dst, BDADDR_ANY);
+
+	while ((opt = getopt_long(argc, argv, "+i:r:s:f:hcFCw:",
+						main_options, NULL)) != EOF) {
+		switch (opt) {
+		case 'i':
+			if (!strncmp(optarg, "hci", 3))
+				hci_devba(atoi(optarg + 3), &src);
+			else
+				str2ba(optarg, &src);
+			break;
+
+		case 'r':
+			mode = MODE_REJECT;
+			cmd = parse_cmd(optarg);
+			break;
+
+		case 'f':
+			invalid = 1;
+			/* Intentionally missing break */
+
+		case 's':
+			mode = MODE_SEND;
+			cmd = parse_cmd(optarg);
+			break;
+
+		case 'c':
+			preconf = 1;
+			break;
+
+		case 'F':
+			fragment = 1;
+			break;
+
+		case 'C':
+			avctp = 1;
+			break;
+
+		case 'w':
+			wait_before_exit = atoi(optarg);
+			break;
+
+		case 'h':
+		default:
+			usage();
+			exit(0);
+		}
+	}
+
+	if (argv[optind])
+		str2ba(argv[optind], &dst);
+
+	if (avctp) {
+		avctp = mode;
+		mode = MODE_SEND;
+	}
+
+	switch (mode) {
+	case MODE_REJECT:
+		do_listen(&src, cmd, fragment);
+		break;
+	case MODE_SEND:
+		sk = do_connect(&src, &dst, avctp, fragment);
+		if (sk < 0)
+			exit(1);
+		if (avctp) {
+			if (avctp == MODE_SEND)
+				do_avctp_send(sk, invalid);
+			else
+				process_avctp(sk, cmd);
+		} else
+			do_avdtp_send(sk, &src, &dst, cmd, invalid, preconf);
+		if (wait_before_exit) {
+			printf("Waiting %d seconds before exiting\n", wait_before_exit);
+			sleep(wait_before_exit);
+		}
+		if (media_sock >= 0)
+			close(media_sock);
+		close(sk);
+		break;
+	default:
+		fprintf(stderr, "No operating mode specified!\n");
+		exit(1);
+	}
+
+	return 0;
+}
diff --git a/test/bdaddr.8 b/test/bdaddr.8
new file mode 100644
index 0000000..88345f8
--- /dev/null
+++ b/test/bdaddr.8
@@ -0,0 +1,68 @@
+.TH BDADDR 8 "Sep 27 2005" BlueZ "Linux System Administration"
+.SH NAME
+bdaddr \- Utility for changing the Bluetooth device address
+.SH SYNOPSIS
+.B bdaddr
+.br
+.B bdaddr -h
+.br
+.B bdaddr [-i <dev>] [-r] [-t] [new bdaddr]
+
+.SH DESCRIPTION
+.LP
+.B
+bdaddr
+is used to query or set the local Bluetooth device address (BD_ADDR). If run
+with no arguments,
+.B
+bdaddr
+prints the chip manufacturer's name, and the current BD_ADDR. If the IEEE OUI
+index file "oui.txt" is installed on the system, the BD_ADDR owner will be
+displayed. If the optional [new bdaddr] argument is given, the device will be
+reprogrammed with that address. This can either be permanent or temporary, as
+specified by the -t flag. In both cases, the device must be reset before the
+new address will become active. This can be done with a 'soft' reset by
+specifying the -r flag, or a 'hard' reset by removing and replugging the
+device. A 'hard' reset will cause the address to revert to the current
+non-volatile value.
+.PP
+.B
+bdaddr
+uses manufacturer specific commands to set the address, and is therefore
+device specific. For this reason, not all devices are supported, and not all
+options are supported on all devices.
+Current supported manufacturers are:
+.B Ericsson, Cambridge Silicon Radio (CSR), Texas Instruments (TI), Zeevo
+and
+.B ST Microelectronics (ST)
+
+.SH OPTIONS
+.TP
+.BI -h
+Gives a list of possible commands.
+.TP
+.BI -i\ <dev>
+Specify a particular device to operate on. If not specified, default is the
+first available device.
+.TP
+.BI -r
+Reset device and make new BD_ADDR active.
+.B
+CSR
+devices only.
+.TP
+.BI -t
+Temporary change. Do not write to non-volatile memory.
+.B
+CSR
+devices only.
+.SH FILES
+.TP
+.I
+/usr/share/misc/oui.txt
+IEEE Organizationally Unique Identifier master file.
+Manually update from: http://standards.ieee.org/regauth/oui/oui.txt
+.SH AUTHORS
+Written by Marcel Holtmann <marcel@holtmann.org>,
+man page by Adam Laurie <adam@algroup.co.uk>
+.PP
diff --git a/test/bdaddr.c b/test/bdaddr.c
new file mode 100644
index 0000000..683b3b9
--- /dev/null
+++ b/test/bdaddr.c
@@ -0,0 +1,460 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+
+#include "oui.h"
+
+static int transient = 0;
+
+static int generic_reset_device(int dd)
+{
+	bdaddr_t bdaddr;
+	int err;
+
+	err = hci_send_cmd(dd, 0x03, 0x0003, 0, NULL);
+	if (err < 0)
+		return err;
+
+	return hci_read_bd_addr(dd, &bdaddr, 10000);
+}
+
+#define OCF_ERICSSON_WRITE_BD_ADDR	0x000d
+typedef struct {
+	bdaddr_t	bdaddr;
+} __attribute__ ((packed)) ericsson_write_bd_addr_cp;
+#define ERICSSON_WRITE_BD_ADDR_CP_SIZE 6
+
+static int ericsson_write_bd_addr(int dd, bdaddr_t *bdaddr)
+{
+	struct hci_request rq;
+	ericsson_write_bd_addr_cp cp;
+
+	memset(&cp, 0, sizeof(cp));
+	bacpy(&cp.bdaddr, bdaddr);
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = OCF_ERICSSON_WRITE_BD_ADDR;
+	rq.cparam = &cp;
+	rq.clen   = ERICSSON_WRITE_BD_ADDR_CP_SIZE;
+	rq.rparam = NULL;
+	rq.rlen   = 0;
+
+	if (hci_send_req(dd, &rq, 1000) < 0)
+		return -1;
+
+	return 0;
+}
+
+#define OCF_ERICSSON_STORE_IN_FLASH	0x0022
+typedef struct {
+	uint8_t		user_id;
+	uint8_t		flash_length;
+	uint8_t		flash_data[253];
+} __attribute__ ((packed)) ericsson_store_in_flash_cp;
+#define ERICSSON_STORE_IN_FLASH_CP_SIZE 255
+
+static int ericsson_store_in_flash(int dd, uint8_t user_id, uint8_t flash_length, uint8_t *flash_data)
+{
+	struct hci_request rq;
+	ericsson_store_in_flash_cp cp;
+
+	memset(&cp, 0, sizeof(cp));
+	cp.user_id = user_id;
+	cp.flash_length = flash_length;
+	if (flash_length > 0)
+		memcpy(cp.flash_data, flash_data, flash_length);
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = OCF_ERICSSON_STORE_IN_FLASH;
+	rq.cparam = &cp;
+	rq.clen   = ERICSSON_STORE_IN_FLASH_CP_SIZE;
+	rq.rparam = NULL;
+	rq.rlen   = 0;
+
+	if (hci_send_req(dd, &rq, 1000) < 0)
+		return -1;
+
+	return 0;
+}
+
+static int csr_write_bd_addr(int dd, bdaddr_t *bdaddr)
+{
+	unsigned char cmd[] = { 0x02, 0x00, 0x0c, 0x00, 0x11, 0x47, 0x03, 0x70,
+				0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00,
+				0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+
+	unsigned char cp[254], rp[254];
+	struct hci_request rq;
+
+	if (transient)
+		cmd[14] = 0x08;
+
+	cmd[16] = bdaddr->b[2];
+	cmd[17] = 0x00;
+	cmd[18] = bdaddr->b[0];
+	cmd[19] = bdaddr->b[1];
+	cmd[20] = bdaddr->b[3];
+	cmd[21] = 0x00;
+	cmd[22] = bdaddr->b[4];
+	cmd[23] = bdaddr->b[5];
+
+	memset(&cp, 0, sizeof(cp));
+	cp[0] = 0xc2;
+	memcpy(cp + 1, cmd, sizeof(cmd));
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = 0x00;
+	rq.event  = EVT_VENDOR;
+	rq.cparam = cp;
+	rq.clen   = sizeof(cmd) + 1;
+	rq.rparam = rp;
+	rq.rlen   = sizeof(rp);
+
+	if (hci_send_req(dd, &rq, 2000) < 0)
+		return -1;
+
+	if (rp[0] != 0xc2) {
+		errno = EIO;
+		return -1;
+	}
+
+	if ((rp[9] + (rp[10] << 8)) != 0) {
+		errno = ENXIO;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int csr_reset_device(int dd)
+{
+	unsigned char cmd[] = { 0x02, 0x00, 0x09, 0x00,
+				0x00, 0x00, 0x01, 0x40, 0x00, 0x00,
+				0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+
+	unsigned char cp[254], rp[254];
+	struct hci_request rq;
+
+	if (transient)
+		cmd[6] = 0x02;
+
+	memset(&cp, 0, sizeof(cp));
+	cp[0] = 0xc2;
+	memcpy(cp + 1, cmd, sizeof(cmd));
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = 0x00;
+	rq.event  = EVT_VENDOR;
+	rq.cparam = cp;
+	rq.clen   = sizeof(cmd) + 1;
+	rq.rparam = rp;
+	rq.rlen   = sizeof(rp);
+
+	if (hci_send_req(dd, &rq, 2000) < 0)
+		return -1;
+
+	return 0;
+}
+
+#define OCF_TI_WRITE_BD_ADDR		0x0006
+typedef struct {
+	bdaddr_t	bdaddr;
+} __attribute__ ((packed)) ti_write_bd_addr_cp;
+#define TI_WRITE_BD_ADDR_CP_SIZE 6
+
+static int ti_write_bd_addr(int dd, bdaddr_t *bdaddr)
+{
+	struct hci_request rq;
+	ti_write_bd_addr_cp cp;
+
+	memset(&cp, 0, sizeof(cp));
+	bacpy(&cp.bdaddr, bdaddr);
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = OCF_TI_WRITE_BD_ADDR;
+	rq.cparam = &cp;
+	rq.clen   = TI_WRITE_BD_ADDR_CP_SIZE;
+	rq.rparam = NULL;
+	rq.rlen   = 0;
+
+	if (hci_send_req(dd, &rq, 1000) < 0)
+		return -1;
+
+	return 0;
+}
+
+#define OCF_BCM_WRITE_BD_ADDR		0x0001
+typedef struct {
+	bdaddr_t	bdaddr;
+} __attribute__ ((packed)) bcm_write_bd_addr_cp;
+#define BCM_WRITE_BD_ADDR_CP_SIZE 6
+
+static int bcm_write_bd_addr(int dd, bdaddr_t *bdaddr)
+{
+	struct hci_request rq;
+	bcm_write_bd_addr_cp cp;
+
+	memset(&cp, 0, sizeof(cp));
+	bacpy(&cp.bdaddr, bdaddr);
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = OCF_BCM_WRITE_BD_ADDR;
+	rq.cparam = &cp;
+	rq.clen   = BCM_WRITE_BD_ADDR_CP_SIZE;
+	rq.rparam = NULL;
+	rq.rlen   = 0;
+
+	if (hci_send_req(dd, &rq, 1000) < 0)
+		return -1;
+
+	return 0;
+}
+
+#define OCF_ZEEVO_WRITE_BD_ADDR		0x0001
+typedef struct {
+	bdaddr_t	bdaddr;
+} __attribute__ ((packed)) zeevo_write_bd_addr_cp;
+#define ZEEVO_WRITE_BD_ADDR_CP_SIZE 6
+
+static int zeevo_write_bd_addr(int dd, bdaddr_t *bdaddr)
+{
+	struct hci_request rq;
+	zeevo_write_bd_addr_cp cp;
+
+	memset(&cp, 0, sizeof(cp));
+	bacpy(&cp.bdaddr, bdaddr);
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = OCF_ZEEVO_WRITE_BD_ADDR;
+	rq.cparam = &cp;
+	rq.clen   = ZEEVO_WRITE_BD_ADDR_CP_SIZE;
+	rq.rparam = NULL;
+	rq.rlen   = 0;
+
+	if (hci_send_req(dd, &rq, 1000) < 0)
+		return -1;
+
+	return 0;
+}
+
+static int st_write_bd_addr(int dd, bdaddr_t *bdaddr)
+{
+	return ericsson_store_in_flash(dd, 0xfe, 6, (uint8_t *) bdaddr);
+}
+
+static struct {
+	uint16_t compid;
+	int (*write_bd_addr)(int dd, bdaddr_t *bdaddr);
+	int (*reset_device)(int dd);
+} vendor[] = {
+	{ 0,		ericsson_write_bd_addr,	NULL			},
+	{ 10,		csr_write_bd_addr,	csr_reset_device	},
+	{ 13,		ti_write_bd_addr,	NULL			},
+	{ 15,		bcm_write_bd_addr,	generic_reset_device	},
+	{ 18,		zeevo_write_bd_addr,	NULL			},
+	{ 48,		st_write_bd_addr,	generic_reset_device	},
+	{ 57,		ericsson_write_bd_addr,	generic_reset_device	},
+	{ 65535,	NULL,			NULL			},
+};
+
+static void usage(void)
+{
+	printf("bdaddr - Utility for changing the Bluetooth device address\n\n");
+	printf("Usage:\n"
+		"\tbdaddr [-i <dev>] [-r] [-t] [new bdaddr]\n");
+}
+
+static struct option main_options[] = {
+	{ "device",	1, 0, 'i' },
+	{ "reset",	0, 0, 'r' },
+	{ "transient",	0, 0, 't' },
+	{ "help",	0, 0, 'h' },
+	{ 0, 0, 0, 0 }
+};
+
+int main(int argc, char *argv[])
+{
+	struct hci_dev_info di;
+	struct hci_version ver;
+	bdaddr_t bdaddr;
+	char addr[18], oui[9], *comp;
+	int i, dd, opt, dev = 0, reset = 0;
+
+	bacpy(&bdaddr, BDADDR_ANY);
+
+	while ((opt=getopt_long(argc, argv, "+i:rth", main_options, NULL)) != -1) {
+		switch (opt) {
+		case 'i':
+			dev = hci_devid(optarg);
+			if (dev < 0) {
+				perror("Invalid device");
+				exit(1);
+			}
+			break;
+
+		case 'r':
+			reset = 1;
+			break;
+
+		case 't':
+			transient = 1;
+			break;
+
+		case 'h':
+		default:
+			usage();
+			exit(0);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+	optind = 0;
+
+	dd = hci_open_dev(dev);
+	if (dd < 0) {
+		fprintf(stderr, "Can't open device hci%d: %s (%d)\n",
+						dev, strerror(errno), errno);
+		exit(1);
+	}
+
+	if (hci_devinfo(dev, &di) < 0) {
+		fprintf(stderr, "Can't get device info for hci%d: %s (%d)\n",
+						dev, strerror(errno), errno);
+		hci_close_dev(dd);
+		exit(1);
+	}
+
+	if (hci_read_local_version(dd, &ver, 1000) < 0) {
+		fprintf(stderr, "Can't read version info for hci%d: %s (%d)\n",
+						dev, strerror(errno), errno);
+		hci_close_dev(dd);
+		exit(1);
+	}
+
+	if (!bacmp(&di.bdaddr, BDADDR_ANY)) {
+		if (hci_read_bd_addr(dd, &bdaddr, 1000) < 0) {
+			fprintf(stderr, "Can't read address for hci%d: %s (%d)\n",
+						dev, strerror(errno), errno);
+			hci_close_dev(dd);
+			exit(1);
+		}
+	} else
+		bacpy(&bdaddr, &di.bdaddr);
+
+	printf("Manufacturer:   %s (%d)\n",
+			bt_compidtostr(ver.manufacturer), ver.manufacturer);
+
+	ba2oui(&bdaddr, oui);
+	comp = ouitocomp(oui);
+
+	ba2str(&bdaddr, addr);
+	printf("Device address: %s", addr);
+
+	if (comp) {
+		printf(" (%s)\n", comp);
+		free(comp);
+	} else
+		printf("\n");
+
+	if (argc < 1) {
+		hci_close_dev(dd);
+		exit(0);
+	}
+
+	str2ba(argv[0], &bdaddr);
+	if (!bacmp(&bdaddr, BDADDR_ANY)) {
+		hci_close_dev(dd);
+		exit(0);
+	}
+
+	for (i = 0; vendor[i].compid != 65535; i++)
+		if (ver.manufacturer == vendor[i].compid) {
+			ba2oui(&bdaddr, oui);
+			comp = ouitocomp(oui);
+
+			ba2str(&bdaddr, addr);
+			printf("New BD address: %s", addr);
+
+			if (comp) {
+				printf(" (%s)\n\n", comp);
+				free(comp);
+			} else
+				printf("\n\n");
+
+
+			if (vendor[i].write_bd_addr(dd, &bdaddr) < 0) {
+				fprintf(stderr, "Can't write new address\n");
+				hci_close_dev(dd);
+				exit(1);
+			}
+
+			printf("Address changed - ");
+
+			if (reset && vendor[i].reset_device) {
+				if (vendor[i].reset_device(dd) < 0) {
+					printf("Reset device manually\n");
+				} else {
+					ioctl(dd, HCIDEVRESET, dev);
+					printf("Device reset successully\n");
+				}
+			} else {
+				printf("Reset device now\n");
+			}
+
+			//ioctl(dd, HCIDEVRESET, dev);
+			//ioctl(dd, HCIDEVDOWN, dev);
+			//ioctl(dd, HCIDEVUP, dev);
+
+			hci_close_dev(dd);
+			exit(0);
+		}
+
+	hci_close_dev(dd);
+
+	printf("\n");
+	fprintf(stderr, "Unsupported manufacturer\n");
+
+	exit(1);
+}
diff --git a/test/btiotest.c b/test/btiotest.c
new file mode 100644
index 0000000..c02a25a
--- /dev/null
+++ b/test/btiotest.c
@@ -0,0 +1,555 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2009-2010  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2009-2010  Nokia Corporation
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+#include <signal.h>
+
+#include <glib.h>
+
+#include "btio.h"
+
+#define DEFAULT_ACCEPT_TIMEOUT 2
+
+struct io_data {
+	guint ref;
+	GIOChannel *io;
+	BtIOType type;
+	gint reject;
+	gint disconn;
+	gint accept;
+};
+
+static void io_data_unref(struct io_data *data)
+{
+	data->ref--;
+
+	if (data->ref)
+		return;
+
+	if (data->io)
+		g_io_channel_unref(data->io);
+
+	g_free(data);
+}
+
+static struct io_data *io_data_ref(struct io_data *data)
+{
+	data->ref++;
+	return data;
+}
+
+static struct io_data *io_data_new(GIOChannel *io, BtIOType type, gint reject,
+						gint disconn, gint accept)
+{
+	struct io_data *data;
+
+	data = g_new0(struct io_data, 1);
+	data->io = io;
+	data->type = type;
+	data->reject = reject;
+	data->disconn = disconn;
+	data->accept = accept;
+
+	return io_data_ref(data);
+}
+
+static gboolean io_watch(GIOChannel *io, GIOCondition cond, gpointer user_data)
+{
+	printf("Disconnected\n");
+	return FALSE;
+}
+
+static gboolean disconn_timeout(gpointer user_data)
+{
+	struct io_data *data = user_data;
+
+	printf("Disconnecting\n");
+
+	g_io_channel_shutdown(data->io, TRUE, NULL);
+
+	return FALSE;
+}
+
+static void connect_cb(GIOChannel *io, GError *err, gpointer user_data)
+{
+	struct io_data *data = user_data;
+	GIOCondition cond;
+	char addr[18];
+	uint16_t handle;
+	uint8_t cls[3];
+
+	if (err) {
+		printf("Connecting failed: %s\n", err->message);
+		return;
+	}
+
+	if (!bt_io_get(io, data->type, &err,
+			BT_IO_OPT_DEST, addr,
+			BT_IO_OPT_HANDLE, &handle,
+			BT_IO_OPT_CLASS, cls,
+			BT_IO_OPT_INVALID)) {
+		printf("Unable to get destination address: %s\n",
+								err->message);
+		g_clear_error(&err);
+		strcpy(addr, "(unknown)");
+	}
+
+	printf("Successfully connected to %s. handle=%u, class=%02x%02x%02x\n",
+			addr, handle, cls[0], cls[1], cls[2]);
+
+	if (data->type == BT_IO_L2CAP || data->type == BT_IO_SCO) {
+		uint16_t omtu, imtu;
+
+		if (!bt_io_get(io, data->type, &err,
+					BT_IO_OPT_OMTU, &omtu,
+					BT_IO_OPT_IMTU, &imtu,
+					BT_IO_OPT_INVALID)) {
+			printf("Unable to get L2CAP MTU sizes: %s\n",
+								err->message);
+			g_clear_error(&err);
+		} else
+			printf("imtu=%u, omtu=%u\n", imtu, omtu);
+	}
+
+	if (data->disconn == 0) {
+		g_io_channel_shutdown(io, TRUE, NULL);
+		printf("Disconnected\n");
+		return;
+	}
+
+	if (data->io == NULL)
+		data->io = g_io_channel_ref(io);
+
+	if (data->disconn > 0) {
+		io_data_ref(data);
+		g_timeout_add_seconds_full(G_PRIORITY_DEFAULT, data->disconn,
+					disconn_timeout, data,
+					(GDestroyNotify) io_data_unref);
+	}
+
+
+	io_data_ref(data);
+	cond = G_IO_NVAL | G_IO_HUP | G_IO_ERR;
+	g_io_add_watch_full(io, G_PRIORITY_DEFAULT, cond, io_watch, data,
+					(GDestroyNotify) io_data_unref);
+}
+
+static gboolean confirm_timeout(gpointer user_data)
+{
+	struct io_data *data = user_data;
+
+	if (data->reject >= 0) {
+		printf("Rejecting connection\n");
+		g_io_channel_shutdown(data->io, TRUE, NULL);
+		return FALSE;
+	}
+
+	printf("Accepting connection\n");
+
+	io_data_ref(data);
+
+	if (!bt_io_accept(data->io, connect_cb, data,
+				(GDestroyNotify) io_data_unref, NULL)) {
+		printf("bt_io_accept() failed\n");
+		io_data_unref(data);
+	}
+
+	return FALSE;
+}
+
+static void confirm_cb(GIOChannel *io, gpointer user_data)
+{
+	char addr[18];
+	struct io_data *data = user_data;
+	GError *err = NULL;
+
+	if (!bt_io_get(io, data->type, &err, BT_IO_OPT_DEST, addr,
+							BT_IO_OPT_INVALID)) {
+		printf("bt_io_get(OPT_DEST): %s\n", err->message);
+		g_clear_error(&err);
+	} else
+		printf("Got confirmation request for %s\n", addr);
+
+	if (data->accept < 0 && data->reject < 0)
+		return;
+
+	if (data->reject == 0) {
+		printf("Rejecting connection\n");
+		g_io_channel_shutdown(io, TRUE, NULL);
+		return;
+	}
+
+	data->io = g_io_channel_ref(io);
+	io_data_ref(data);
+
+	if (data->accept == 0) {
+		if (!bt_io_accept(io, connect_cb, data,
+					(GDestroyNotify) io_data_unref,
+					&err)) {
+			printf("bt_io_accept() failed: %s\n", err->message);
+			g_clear_error(&err);
+			io_data_unref(data);
+			return;
+		}
+	} else {
+		gint seconds = (data->reject > 0) ?
+						data->reject : data->accept;
+		g_timeout_add_seconds_full(G_PRIORITY_DEFAULT, seconds,
+					confirm_timeout, data,
+					(GDestroyNotify) io_data_unref);
+	}
+}
+
+static void l2cap_connect(const char *src, const char *dst, uint16_t psm,
+						gint disconn, gint sec)
+{
+	struct io_data *data;
+	GError *err = NULL;
+
+	printf("Connecting to %s L2CAP PSM %u\n", dst, psm);
+
+	data = io_data_new(NULL, BT_IO_L2CAP, -1, disconn, -1);
+
+	if (src)
+		data->io = bt_io_connect(BT_IO_L2CAP, connect_cb, data,
+						(GDestroyNotify) io_data_unref,
+						&err,
+						BT_IO_OPT_SOURCE, src,
+						BT_IO_OPT_DEST, dst,
+						BT_IO_OPT_PSM, psm,
+						BT_IO_OPT_SEC_LEVEL, sec,
+						BT_IO_OPT_INVALID);
+	else
+		data->io = bt_io_connect(BT_IO_L2CAP, connect_cb, data,
+						(GDestroyNotify) io_data_unref,
+						&err,
+						BT_IO_OPT_DEST, dst,
+						BT_IO_OPT_PSM, psm,
+						BT_IO_OPT_SEC_LEVEL, sec,
+						BT_IO_OPT_INVALID);
+
+	if (!data->io) {
+		printf("Connecting to %s failed: %s\n", dst, err->message);
+		g_error_free(err);
+		exit(EXIT_FAILURE);
+	}
+}
+
+static void l2cap_listen(const char *src, uint16_t psm, gint defer,
+				gint reject, gint disconn, gint accept,
+				gint sec, gboolean master)
+{
+	struct io_data *data;
+	BtIOConnect conn;
+	BtIOConfirm cfm;
+	GIOChannel *l2_srv;
+	GError *err = NULL;
+
+	if (defer) {
+		conn = NULL;
+		cfm = confirm_cb;
+	} else {
+		conn = connect_cb;
+		cfm = NULL;
+	}
+
+	printf("Listening on L2CAP PSM %u\n", psm);
+
+	data = io_data_new(NULL, BT_IO_L2CAP, reject, disconn, accept);
+
+	if (src)
+		l2_srv = bt_io_listen(BT_IO_L2CAP, conn, cfm,
+					data, (GDestroyNotify) io_data_unref,
+					&err,
+					BT_IO_OPT_SOURCE, src,
+					BT_IO_OPT_PSM, psm,
+					BT_IO_OPT_SEC_LEVEL, sec,
+					BT_IO_OPT_MASTER, master,
+					BT_IO_OPT_INVALID);
+	else
+		l2_srv = bt_io_listen(BT_IO_L2CAP, conn, cfm,
+					data, (GDestroyNotify) io_data_unref,
+					&err,
+					BT_IO_OPT_PSM, psm,
+					BT_IO_OPT_SEC_LEVEL, sec,
+					BT_IO_OPT_MASTER, master,
+					BT_IO_OPT_INVALID);
+
+	if (!l2_srv) {
+		printf("Listening failed: %s\n", err->message);
+		g_error_free(err);
+		exit(EXIT_FAILURE);
+	}
+
+	g_io_channel_unref(l2_srv);
+}
+
+static void rfcomm_connect(const char *src, const char *dst, uint8_t ch,
+						gint disconn, gint sec)
+{
+	struct io_data *data;
+	GError *err = NULL;
+
+	printf("Connecting to %s RFCOMM channel %u\n", dst, ch);
+
+	data = io_data_new(NULL, BT_IO_RFCOMM, -1, disconn, -1);
+
+	if (src)
+		data->io = bt_io_connect(BT_IO_RFCOMM, connect_cb, data,
+						(GDestroyNotify) io_data_unref,
+						&err,
+						BT_IO_OPT_SOURCE, src,
+						BT_IO_OPT_DEST, dst,
+						BT_IO_OPT_CHANNEL, ch,
+						BT_IO_OPT_SEC_LEVEL, sec,
+						BT_IO_OPT_INVALID);
+	else
+		data->io = bt_io_connect(BT_IO_RFCOMM, connect_cb, data,
+						(GDestroyNotify) io_data_unref,
+						&err,
+						BT_IO_OPT_DEST, dst,
+						BT_IO_OPT_CHANNEL, ch,
+						BT_IO_OPT_SEC_LEVEL, sec,
+						BT_IO_OPT_INVALID);
+
+	if (!data->io) {
+		printf("Connecting to %s failed: %s\n", dst, err->message);
+		g_error_free(err);
+		exit(EXIT_FAILURE);
+	}
+}
+
+static void rfcomm_listen(const char *src, uint8_t ch, gboolean defer,
+				gint reject, gint disconn, gint accept,
+				gint sec, gboolean master)
+{
+	struct io_data *data;
+	BtIOConnect conn;
+	BtIOConfirm cfm;
+	GIOChannel *rc_srv;
+	GError *err = NULL;
+
+	if (defer) {
+		conn = NULL;
+		cfm = confirm_cb;
+	} else {
+		conn = connect_cb;
+		cfm = NULL;
+	}
+
+	data = io_data_new(NULL, BT_IO_RFCOMM, reject, disconn, accept);
+
+	if (src)
+		rc_srv = bt_io_listen(BT_IO_RFCOMM, conn, cfm,
+					data, (GDestroyNotify) io_data_unref,
+					&err,
+					BT_IO_OPT_SOURCE, src,
+					BT_IO_OPT_CHANNEL, ch,
+					BT_IO_OPT_SEC_LEVEL, sec,
+					BT_IO_OPT_MASTER, master,
+					BT_IO_OPT_INVALID);
+	else
+		rc_srv = bt_io_listen(BT_IO_RFCOMM, conn, cfm,
+					data, (GDestroyNotify) io_data_unref,
+					&err,
+					BT_IO_OPT_CHANNEL, ch,
+					BT_IO_OPT_SEC_LEVEL, sec,
+					BT_IO_OPT_MASTER, master,
+					BT_IO_OPT_INVALID);
+
+	if (!rc_srv) {
+		printf("Listening failed: %s\n", err->message);
+		g_error_free(err);
+		exit(EXIT_FAILURE);
+	}
+
+	bt_io_get(rc_srv, BT_IO_RFCOMM, &err,
+			BT_IO_OPT_CHANNEL, &ch,
+			BT_IO_OPT_INVALID);
+
+	printf("Listening on RFCOMM channel %u\n", ch);
+
+	g_io_channel_unref(rc_srv);
+}
+
+static void sco_connect(const char *src, const char *dst, gint disconn)
+{
+	struct io_data *data;
+	GError *err = NULL;
+
+	printf("Connecting SCO to %s\n", dst);
+
+	data = io_data_new(NULL, BT_IO_SCO, -1, disconn, -1);
+
+	if (src)
+		data->io = bt_io_connect(BT_IO_SCO, connect_cb, data,
+						(GDestroyNotify) io_data_unref,
+						&err,
+						BT_IO_OPT_SOURCE, src,
+						BT_IO_OPT_DEST, dst,
+						BT_IO_OPT_INVALID);
+	else
+		data->io = bt_io_connect(BT_IO_SCO, connect_cb, data,
+						(GDestroyNotify) io_data_unref,
+						&err,
+						BT_IO_OPT_DEST, dst,
+						BT_IO_OPT_INVALID);
+
+	if (!data->io) {
+		printf("Connecting to %s failed: %s\n", dst, err->message);
+		g_error_free(err);
+		exit(EXIT_FAILURE);
+	}
+}
+
+static void sco_listen(const char *src, gint disconn)
+{
+	struct io_data *data;
+	GIOChannel *sco_srv;
+	GError *err = NULL;
+
+	printf("Listening for SCO connections\n");
+
+	data = io_data_new(NULL, BT_IO_SCO, -1, disconn, -1);
+
+	if (src)
+		sco_srv = bt_io_listen(BT_IO_SCO, connect_cb, NULL,
+					data, (GDestroyNotify) io_data_unref,
+					&err,
+					BT_IO_OPT_SOURCE, src,
+					BT_IO_OPT_INVALID);
+	else
+		sco_srv = bt_io_listen(BT_IO_SCO, connect_cb, NULL,
+					data, (GDestroyNotify) io_data_unref,
+					&err, BT_IO_OPT_INVALID);
+
+	if (!sco_srv) {
+		printf("Listening failed: %s\n", err->message);
+		g_error_free(err);
+		exit(EXIT_FAILURE);
+	}
+
+	g_io_channel_unref(sco_srv);
+}
+
+static gint opt_channel = -1;
+static gint opt_psm = 0;
+static gboolean opt_sco = FALSE;
+static gboolean opt_defer = FALSE;
+static char *opt_dev = NULL;
+static gint opt_reject = -1;
+static gint opt_disconn = -1;
+static gint opt_accept = DEFAULT_ACCEPT_TIMEOUT;
+static gint opt_sec = 0;
+static gboolean opt_master = FALSE;
+
+static GMainLoop *main_loop;
+
+static GOptionEntry options[] = {
+	{ "channel", 'c', 0, G_OPTION_ARG_INT, &opt_channel,
+				"RFCOMM channel" },
+	{ "psm", 'p', 0, G_OPTION_ARG_INT, &opt_psm,
+				"L2CAP PSM" },
+	{ "sco", 's', 0, G_OPTION_ARG_NONE, &opt_sco,
+				"Use SCO" },
+	{ "defer", 'd', 0, G_OPTION_ARG_NONE, &opt_defer,
+				"Use DEFER_SETUP for incoming connections" },
+	{ "sec-level", 'S', 0, G_OPTION_ARG_INT, &opt_sec,
+				"Security level" },
+	{ "dev", 'i', 0, G_OPTION_ARG_STRING, &opt_dev,
+				"Which HCI device to use" },
+	{ "reject", 'r', 0, G_OPTION_ARG_INT, &opt_reject,
+				"Reject connection after N seconds" },
+	{ "disconnect", 'D', 0, G_OPTION_ARG_INT, &opt_disconn,
+				"Disconnect connection after N seconds" },
+	{ "accept", 'a', 0, G_OPTION_ARG_INT, &opt_accept,
+				"Accept connection after N seconds" },
+	{ "master", 'm', 0, G_OPTION_ARG_NONE, &opt_master,
+				"Master role switch (incoming connections)" },
+	{ NULL },
+};
+
+static void sig_term(int sig)
+{
+	g_main_loop_quit(main_loop);
+}
+
+int main(int argc, char *argv[])
+{
+	GOptionContext *context;
+
+	context = g_option_context_new(NULL);
+	g_option_context_add_main_entries(context, options, NULL);
+
+	if (!g_option_context_parse(context, &argc, &argv, NULL))
+		exit(EXIT_FAILURE);
+
+	g_option_context_free(context);
+
+	printf("accept=%d, reject=%d, discon=%d, defer=%d, sec=%d\n",
+		opt_accept, opt_reject, opt_disconn, opt_defer, opt_sec);
+
+	if (opt_psm) {
+		if (argc > 1)
+			l2cap_connect(opt_dev, argv[1], opt_psm,
+							opt_disconn, opt_sec);
+		else
+			l2cap_listen(opt_dev, opt_psm, opt_defer, opt_reject,
+					opt_disconn, opt_accept, opt_sec,
+					opt_master);
+	}
+
+	if (opt_channel != -1) {
+		if (argc > 1)
+			rfcomm_connect(opt_dev, argv[1], opt_channel,
+							opt_disconn, opt_sec);
+		else
+			rfcomm_listen(opt_dev, opt_channel, opt_defer,
+					opt_reject, opt_disconn, opt_accept,
+					opt_sec, opt_master);
+	}
+
+	if (opt_sco) {
+		if (argc > 1)
+			sco_connect(opt_dev, argv[1], opt_disconn);
+		else
+			sco_listen(opt_dev, opt_disconn);
+	}
+
+	signal(SIGTERM, sig_term);
+	signal(SIGINT, sig_term);
+
+	main_loop = g_main_loop_new(NULL, FALSE);
+
+	g_main_loop_run(main_loop);
+
+	g_main_loop_unref(main_loop);
+
+	printf("Exiting\n");
+
+	exit(EXIT_SUCCESS);
+}
diff --git a/test/dbusdef.py b/test/dbusdef.py
new file mode 100644
index 0000000..5af6153
--- /dev/null
+++ b/test/dbusdef.py
@@ -0,0 +1,16 @@
+import dbus
+
+bus = dbus.SystemBus()
+
+
+dummy = dbus.Interface(bus.get_object('org.bluez', '/'), 'org.freedesktop.DBus.Introspectable')
+
+#print dummy.Introspect()
+
+
+manager = dbus.Interface(bus.get_object('org.bluez', '/'), 'org.bluez.Manager')
+
+try:
+	adapter = dbus.Interface(bus.get_object('org.bluez', manager.DefaultAdapter()), 'org.bluez.Adapter')
+except:
+	pass
diff --git a/test/gaptest.c b/test/gaptest.c
new file mode 100644
index 0000000..3e9f534
--- /dev/null
+++ b/test/gaptest.c
@@ -0,0 +1,335 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2007-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+
+#include <dbus/dbus.h>
+
+#define BLUEZ_SERVICE	"org.bluez"
+
+#define MANAGER_PATH	"/"
+#define MANAGER_INTF	BLUEZ_SERVICE ".Manager"
+#define ADAPTER_INTF	BLUEZ_SERVICE ".Adapter"
+
+static char *get_adapter(DBusConnection *conn)
+{
+	DBusMessage *message, *reply;
+	DBusError error;
+	const char *path;
+	char *result = NULL;
+
+	message = dbus_message_new_method_call(BLUEZ_SERVICE, MANAGER_PATH,
+					MANAGER_INTF, "DefaultAdapter");
+	if (!message)
+		return NULL;
+
+	dbus_error_init(&error);
+
+	reply = dbus_connection_send_with_reply_and_block(conn,
+							message, -1, &error);
+
+	dbus_message_unref(message);
+
+	if (!reply) {
+		if (dbus_error_is_set(&error) == TRUE) {
+			fprintf(stderr, "%s\n", error.message);
+			dbus_error_free(&error);
+		} else
+			fprintf(stderr, "Failed to set property\n");
+		return NULL;
+	}
+
+	if (dbus_message_get_args(reply, NULL, DBUS_TYPE_OBJECT_PATH, &path,
+						DBUS_TYPE_INVALID) == FALSE)
+		goto done;
+
+	printf("Using default adapter %s\n", path);
+
+	result = strdup(path);
+
+done:
+	dbus_message_unref(reply);
+
+	return result;
+}
+
+static char *find_device(DBusConnection *conn, const char *adapter,
+							const char *address)
+{
+	DBusMessage *message, *reply;
+	DBusError error;
+	const char *path;
+	char *result = NULL;
+
+	message = dbus_message_new_method_call(BLUEZ_SERVICE, adapter,
+					ADAPTER_INTF, "FindDevice");
+	if (!message)
+		return NULL;
+
+	dbus_message_append_args(message, DBUS_TYPE_STRING, &address,
+							DBUS_TYPE_INVALID);
+
+	dbus_error_init(&error);
+
+	reply = dbus_connection_send_with_reply_and_block(conn,
+							message, -1, &error);
+
+	dbus_message_unref(message);
+
+	if (!reply) {
+		if (dbus_error_is_set(&error) == TRUE) {
+			fprintf(stderr, "%s\n", error.message);
+			dbus_error_free(&error);
+		} else
+			fprintf(stderr, "Failed to set property\n");
+		return NULL;
+	}
+
+	if (dbus_message_get_args(reply, NULL, DBUS_TYPE_OBJECT_PATH, &path,
+						DBUS_TYPE_INVALID) == FALSE)
+		goto done;
+
+	printf("Using device %s for address %s\n", path, address);
+
+	result = strdup(path);
+
+done:
+	dbus_message_unref(reply);
+
+	return result;
+}
+
+static int remove_device(DBusConnection *conn, const char *adapter,
+							const char *device)
+{
+	DBusMessage *message, *reply;
+	DBusError error;
+
+	message = dbus_message_new_method_call(BLUEZ_SERVICE, adapter,
+					ADAPTER_INTF, "RemoveDevice");
+	if (!message)
+		return -ENOMEM;
+
+	dbus_message_append_args(message, DBUS_TYPE_OBJECT_PATH, &device,
+							DBUS_TYPE_INVALID);
+
+	dbus_error_init(&error);
+
+	reply = dbus_connection_send_with_reply_and_block(conn,
+							message, -1, &error);
+
+	dbus_message_unref(message);
+
+	if (!reply) {
+		if (dbus_error_is_set(&error) == TRUE) {
+			fprintf(stderr, "%s\n", error.message);
+			dbus_error_free(&error);
+		} else
+			fprintf(stderr, "Failed to set property\n");
+		return -EIO;
+	}
+
+	dbus_message_unref(reply);
+
+	printf("Removed device %s\n", device);
+
+	return 0;
+}
+
+static int set_property(DBusConnection *conn, const char *adapter,
+					const char *key, int type, void *val)
+{
+	DBusMessage *message, *reply;
+	DBusMessageIter array, value;
+	DBusError error;
+	const char *signature;
+
+	message = dbus_message_new_method_call(BLUEZ_SERVICE, adapter,
+						ADAPTER_INTF, "SetProperty");
+	if (!message)
+		return -ENOMEM;
+
+	switch (type) {
+	case DBUS_TYPE_BOOLEAN:
+		signature = DBUS_TYPE_BOOLEAN_AS_STRING;
+		break;
+	case DBUS_TYPE_UINT32:
+		signature = DBUS_TYPE_UINT32_AS_STRING;
+		break;
+	default:
+		return -EILSEQ;
+	}
+
+	dbus_message_iter_init_append(message, &array);
+
+	dbus_message_iter_append_basic(&array, DBUS_TYPE_STRING, &key);
+
+	dbus_message_iter_open_container(&array, DBUS_TYPE_VARIANT,
+							signature, &value);
+	dbus_message_iter_append_basic(&value, type, val);
+	dbus_message_iter_close_container(&array, &value);
+
+	dbus_error_init(&error);
+
+	reply = dbus_connection_send_with_reply_and_block(conn,
+							message, -1, &error);
+
+	dbus_message_unref(message);
+
+	if (!reply) {
+		if (dbus_error_is_set(&error) == TRUE) {
+			fprintf(stderr, "%s\n", error.message);
+			dbus_error_free(&error);
+		} else
+			fprintf(stderr, "Failed to set property\n");
+		return -EIO;
+	}
+
+	dbus_message_unref(reply);
+
+	printf("Set property %s for %s\n", key, adapter);
+
+	return 0;
+}
+
+static void usage(void)
+{
+	printf("gaptest - GAP testing\n"
+		"Usage:\n");
+	printf("\tgaptest [options]\n");
+	printf("Options:\n"
+		"\t-T <timeout>        Set timeout\n"
+		"\t-P <powered>        Set powered\n"
+		"\t-D <discoverable>   Set discoverable\n"
+		"\t-B <pairable>       Set pairable\n"
+		"\t-C <address>        Create device\n"
+		"\t-R <address>        Remove device\n");
+}
+
+int main(int argc, char *argv[])
+{
+	DBusConnection *conn;
+	char *adapter, *device;
+	const char *create = NULL, *remove = NULL;
+	int opt, timeout = -1, powered = -1, discoverable = -1, pairable = -1;
+
+	while ((opt = getopt(argc, argv, "T:P:D:B:C:R:h")) != EOF) {
+		switch (opt) {
+		case 'T':
+			timeout = atoi(optarg);
+			break;
+		case 'P':
+			powered = atoi(optarg);
+			break;
+		case 'D':
+			discoverable = atoi(optarg);
+			break;
+		case 'B':
+			pairable = atoi(optarg);
+			break;
+		case 'C':
+			create = optarg;
+			break;
+		case 'R':
+			remove = optarg;
+			break;
+		case 'h':
+			usage();
+			exit(0);
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	conn = dbus_bus_get(DBUS_BUS_SYSTEM, NULL);
+	if (!conn) {
+		fprintf(stderr, "Can't get on system bus\n");
+		exit(1);
+	}
+
+	adapter = get_adapter(conn);
+	if (!adapter) {
+		fprintf(stderr, "Can't get default adapter\n");
+		exit(1);
+	}
+
+	if (powered >= 0) {
+		set_property(conn, adapter, "Powered",
+					DBUS_TYPE_BOOLEAN, &powered);
+	}
+
+	if (discoverable >= 0) {
+		set_property(conn, adapter, "Discoverable",
+					DBUS_TYPE_BOOLEAN, &discoverable);
+
+		if (timeout >= 0)
+			set_property(conn, adapter, "DiscoverableTimeout",
+						DBUS_TYPE_UINT32, &timeout);
+	}
+
+	if (pairable >= 0) {
+		set_property(conn, adapter, "Pairable",
+					DBUS_TYPE_BOOLEAN, &pairable);
+
+		if (timeout >= 0)
+			set_property(conn, adapter, "PairableTimeout",
+						DBUS_TYPE_UINT32, &timeout);
+	}
+
+	if (create) {
+		device = find_device(conn, adapter, create);
+		if (!device) {
+			fprintf(stderr, "Can't find device\n");
+			exit(1);
+		}
+
+		free(device);
+	}
+
+	if (remove) {
+		device = find_device(conn, adapter, remove);
+		if (!device) {
+			fprintf(stderr, "Can't find device\n");
+			exit(1);
+		}
+
+		remove_device(conn, adapter, device);
+
+		free(device);
+	}
+
+	free(adapter);
+
+	dbus_connection_unref(conn);
+
+	return 0;
+}
diff --git a/test/hciemu.1 b/test/hciemu.1
new file mode 100644
index 0000000..cecaeb7
--- /dev/null
+++ b/test/hciemu.1
@@ -0,0 +1,31 @@
+.TH HCIEMU 1 "Jul 6 2009" BlueZ ""
+.SH NAME
+hciemu \- HCI emulator
+.SH SYNOPSIS
+.B hciemu
+[\fIoptions\fR] \fIlocal_address\fR
+
+.SH DESCRIPTION
+.LP
+.B
+hciemu
+is used to emulate an HCI via \fBhci_vhci\fR kernel module
+
+.SH OPTIONS
+.TP
+.BI -d\  device
+use specified \fIdevice\fR
+.TP
+.BI -b\  bdaddr
+emulate \fIbdaddr\fR
+.TP
+.BI -s\  file
+create snoop file \fIfile\fR
+.TP
+.B -n
+do not detach
+
+.SH AUTHORS
+Written by Marcel Holtmann <marcel@holtmann.org> and Maxim Krasnyansky
+<maxk@qualcomm.com>, man page by Filippo Giunchedi <filippo@debian.org>
+.PP
diff --git a/test/hciemu.c b/test/hciemu.c
new file mode 100644
index 0000000..9950372
--- /dev/null
+++ b/test/hciemu.c
@@ -0,0 +1,1343 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2000-2002  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2003-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <ctype.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <signal.h>
+#include <getopt.h>
+#include <syslog.h>
+#include <sys/time.h>
+#include <sys/stat.h>
+#include <sys/poll.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <sys/resource.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+
+#include <netdb.h>
+
+#include <glib.h>
+
+#define GHCI_DEV		"/dev/ghci"
+
+#define VHCI_DEV		"/dev/vhci"
+#define VHCI_UDEV		"/dev/hci_vhci"
+
+#define VHCI_MAX_CONN		12
+
+#define VHCI_ACL_MTU		192
+#define VHCI_ACL_MAX_PKT	8
+
+struct vhci_device {
+	uint8_t		features[8];
+	uint8_t		name[248];
+	uint8_t		dev_class[3];
+	uint8_t		inq_mode;
+	uint8_t		eir_fec;
+	uint8_t		eir_data[240];
+	uint16_t	acl_cnt;
+	bdaddr_t	bdaddr;
+	int		fd;
+	int		dd;
+	GIOChannel	*scan;
+};
+
+struct vhci_conn {
+	bdaddr_t	dest;
+	uint16_t	handle;
+	GIOChannel	*chan;
+};
+
+struct vhci_link_info {
+	bdaddr_t	bdaddr;
+	uint8_t		dev_class[3];
+	uint8_t		link_type;
+	uint8_t		role;
+} __attribute__ ((packed));
+
+static struct vhci_device vdev;
+static struct vhci_conn *vconn[VHCI_MAX_CONN];
+
+struct btsnoop_hdr {
+	uint8_t		id[8];		/* Identification Pattern */
+	uint32_t	version;	/* Version Number = 1 */
+	uint32_t	type;		/* Datalink Type */
+} __attribute__ ((packed));
+#define BTSNOOP_HDR_SIZE (sizeof(struct btsnoop_hdr))
+
+struct btsnoop_pkt {
+	uint32_t	size;		/* Original Length */
+	uint32_t	len;		/* Included Length */
+	uint32_t	flags;		/* Packet Flags */
+	uint32_t	drops;		/* Cumulative Drops */
+	uint64_t	ts;		/* Timestamp microseconds */
+	uint8_t		data[0];	/* Packet Data */
+} __attribute__ ((packed));
+#define BTSNOOP_PKT_SIZE (sizeof(struct btsnoop_pkt))
+
+static uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e, 0x6f, 0x6f, 0x70, 0x00 };
+
+static GMainLoop *event_loop;
+
+static volatile sig_atomic_t __io_canceled;
+
+static inline void io_init(void)
+{
+	__io_canceled = 0;
+}
+
+static inline void io_cancel(void)
+{
+	__io_canceled = 1;
+}
+
+static void sig_term(int sig)
+{
+	io_cancel();
+	g_main_loop_quit(event_loop);
+}
+
+static gboolean io_acl_data(GIOChannel *chan, GIOCondition cond, gpointer data);
+static gboolean io_conn_ind(GIOChannel *chan, GIOCondition cond, gpointer data);
+static gboolean io_hci_data(GIOChannel *chan, GIOCondition cond, gpointer data);
+
+static inline int read_n(int fd, void *buf, int len)
+{
+	register int w, t = 0;
+
+	while (!__io_canceled && len > 0) {
+		if ((w = read(fd, buf, len)) < 0 ){
+			if( errno == EINTR || errno == EAGAIN )
+				continue;
+			return -1;
+		}
+		if (!w)
+			return 0;
+		len -= w; buf += w; t += w;
+	}
+	return t;
+}
+
+/* Write exactly len bytes (Signal safe)*/
+static inline int write_n(int fd, void *buf, int len)
+{
+	register int w, t = 0;
+
+	while (!__io_canceled && len > 0) {
+		if ((w = write(fd, buf, len)) < 0 ){
+			if( errno == EINTR || errno == EAGAIN )
+				continue;
+			return -1;
+		}
+		if (!w)
+			return 0;
+		len -= w; buf += w; t += w;
+	}
+	return t;
+}
+
+static int create_snoop(char *file)
+{
+	struct btsnoop_hdr hdr;
+	int fd, len;
+
+	fd = open(file, O_WRONLY | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+	if (fd < 0)
+		return fd;
+
+	memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id));
+	hdr.version = htonl(1);
+	hdr.type = htonl(1002);
+
+	len = write(fd, &hdr, BTSNOOP_HDR_SIZE);
+	if (len < 0) {
+		close(fd);
+		return -EIO;
+	}
+
+	if (len != BTSNOOP_HDR_SIZE) {
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+static int write_snoop(int fd, int type, int incoming, unsigned char *buf, int len)
+{
+	struct btsnoop_pkt pkt;
+	struct timeval tv;
+	uint32_t size = len;
+	uint64_t ts;
+	int err;
+
+	if (fd < 0)
+		return -1;
+
+	memset(&tv, 0, sizeof(tv));
+	gettimeofday(&tv, NULL);
+	ts = (tv.tv_sec - 946684800ll) * 1000000ll + tv.tv_usec;
+
+	pkt.size = htonl(size);
+	pkt.len  = pkt.size;
+	pkt.flags = ntohl(incoming & 0x01);
+	pkt.drops = htonl(0);
+	pkt.ts = hton64(ts + 0x00E03AB44A676000ll);
+
+	if (type == HCI_COMMAND_PKT || type == HCI_EVENT_PKT)
+		pkt.flags |= ntohl(0x02);
+
+	err = write(fd, &pkt, BTSNOOP_PKT_SIZE);
+	err = write(fd, buf, size);
+
+	return 0;
+}
+
+static struct vhci_conn *conn_get_by_bdaddr(bdaddr_t *ba)
+{
+	register int i;
+
+	for (i = 0; i < VHCI_MAX_CONN; i++)
+		if (!bacmp(&vconn[i]->dest, ba))
+			return vconn[i];
+
+	return NULL;
+}
+
+static void command_status(uint16_t ogf, uint16_t ocf, uint8_t status)
+{
+	uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf;
+	evt_cmd_status *cs;
+	hci_event_hdr *he;
+
+	/* Packet type */
+	*ptr++ = HCI_EVENT_PKT;
+
+	/* Event header */
+	he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE;
+
+	he->evt  = EVT_CMD_STATUS;
+	he->plen = EVT_CMD_STATUS_SIZE;
+
+	cs = (void *) ptr; ptr += EVT_CMD_STATUS_SIZE;
+
+	cs->status = status;
+	cs->ncmd   = 1;
+	cs->opcode = htobs(cmd_opcode_pack(ogf, ocf));
+
+	write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf);
+
+	if (write(vdev.fd, buf, ptr - buf) < 0)
+		syslog(LOG_ERR, "Can't send event: %s(%d)",
+						strerror(errno), errno);
+}
+
+static void command_complete(uint16_t ogf, uint16_t ocf, int plen, void *data)
+{
+	uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf;
+	evt_cmd_complete *cc;
+	hci_event_hdr *he;
+
+	/* Packet type */
+	*ptr++ = HCI_EVENT_PKT;
+
+	/* Event header */
+	he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE;
+
+	he->evt  = EVT_CMD_COMPLETE;
+	he->plen = EVT_CMD_COMPLETE_SIZE + plen;
+
+	cc = (void *) ptr; ptr += EVT_CMD_COMPLETE_SIZE;
+
+	cc->ncmd = 1;
+	cc->opcode = htobs(cmd_opcode_pack(ogf, ocf));
+
+	if (plen) {
+		memcpy(ptr, data, plen);
+		ptr += plen;
+	}
+
+	write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf);
+
+	if (write(vdev.fd, buf, ptr - buf) < 0)
+		syslog(LOG_ERR, "Can't send event: %s(%d)",
+						strerror(errno), errno);
+}
+
+static void connect_request(struct vhci_conn *conn)
+{
+	uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf;
+	evt_conn_request *cr;
+	hci_event_hdr *he;
+
+	/* Packet type */
+	*ptr++ = HCI_EVENT_PKT;
+
+	/* Event header */
+	he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE;
+
+	he->evt  = EVT_CONN_REQUEST;
+	he->plen = EVT_CONN_REQUEST_SIZE;
+
+	cr = (void *) ptr; ptr += EVT_CONN_REQUEST_SIZE;
+
+	bacpy(&cr->bdaddr, &conn->dest);
+	memset(&cr->dev_class, 0, sizeof(cr->dev_class));
+	cr->link_type = ACL_LINK;
+
+	write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf);
+
+	if (write(vdev.fd, buf, ptr - buf) < 0)
+		syslog(LOG_ERR, "Can't send event: %s (%d)",
+						strerror(errno), errno);
+}
+
+static void connect_complete(struct vhci_conn *conn)
+{
+	uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf;
+	evt_conn_complete *cc;
+	hci_event_hdr *he;
+
+	/* Packet type */
+	*ptr++ = HCI_EVENT_PKT;
+
+	/* Event header */
+	he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE;
+
+	he->evt  = EVT_CONN_COMPLETE;
+	he->plen = EVT_CONN_COMPLETE_SIZE;
+
+	cc = (void *) ptr; ptr += EVT_CONN_COMPLETE_SIZE;
+
+	bacpy(&cc->bdaddr, &conn->dest);
+	cc->status = 0x00;
+	cc->handle = htobs(conn->handle);
+	cc->link_type = ACL_LINK;
+	cc->encr_mode = 0x00;
+
+	write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf);
+
+	if (write(vdev.fd, buf, ptr - buf) < 0)
+		syslog(LOG_ERR, "Can't send event: %s (%d)",
+						strerror(errno), errno);
+}
+
+static void disconn_complete(struct vhci_conn *conn)
+{
+	uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf;
+	evt_disconn_complete *dc;
+	hci_event_hdr *he;
+
+	/* Packet type */
+	*ptr++ = HCI_EVENT_PKT;
+
+	/* Event header */
+	he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE;
+
+	he->evt  = EVT_DISCONN_COMPLETE;
+	he->plen = EVT_DISCONN_COMPLETE_SIZE;
+
+	dc = (void *) ptr; ptr += EVT_DISCONN_COMPLETE_SIZE;
+
+	dc->status = 0x00;
+	dc->handle = htobs(conn->handle);
+	dc->reason = 0x00;
+
+	write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf);
+
+	if (write(vdev.fd, buf, ptr - buf) < 0)
+		syslog(LOG_ERR, "Can't send event: %s (%d)",
+						strerror(errno), errno);
+
+	vdev.acl_cnt = 0;
+}
+
+static void num_completed_pkts(struct vhci_conn *conn)
+{
+	uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf;
+	evt_num_comp_pkts *np;
+	hci_event_hdr *he;
+
+	/* Packet type */
+	*ptr++ = HCI_EVENT_PKT;
+
+	/* Event header */
+	he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE;
+
+	he->evt  = EVT_NUM_COMP_PKTS;
+	he->plen = EVT_NUM_COMP_PKTS_SIZE;
+
+	np = (void *) ptr; ptr += EVT_NUM_COMP_PKTS_SIZE;
+	np->num_hndl = 1;
+
+	*((uint16_t *) ptr) = htobs(conn->handle); ptr += 2;
+	*((uint16_t *) ptr) = htobs(vdev.acl_cnt); ptr += 2;
+
+	write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf);
+
+	if (write(vdev.fd, buf, ptr - buf) < 0)
+		syslog(LOG_ERR, "Can't send event: %s (%d)",
+						strerror(errno), errno);
+}
+
+static int scan_enable(uint8_t *data)
+{
+	struct sockaddr_in sa;
+	GIOChannel *sk_io;
+	bdaddr_t ba;
+	int sk, opt;
+
+	if (!(*data & SCAN_PAGE)) {
+		if (vdev.scan) {
+			g_io_channel_shutdown(vdev.scan, TRUE, NULL);
+			vdev.scan = NULL;
+		}
+		return 0;
+	}
+
+	if (vdev.scan)
+		return 0;
+
+	if ((sk = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+		syslog(LOG_ERR, "Can't create socket: %s (%d)",
+						strerror(errno), errno);
+		return 1;
+	}
+
+	opt = 1;
+	setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+
+	baswap(&ba, &vdev.bdaddr);
+	sa.sin_family = AF_INET;
+	memcpy(&sa.sin_addr.s_addr, &ba, sizeof(sa.sin_addr.s_addr));
+	sa.sin_port = *(uint16_t *) &ba.b[4];
+	if (bind(sk, (struct sockaddr *) &sa, sizeof(sa))) {
+		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
+						strerror(errno), errno);
+		goto failed;
+	}
+
+	if (listen(sk, 10)) {
+		syslog(LOG_ERR, "Can't listen on socket: %s (%d)",
+						strerror(errno), errno);
+		goto failed;
+	}
+
+	sk_io = g_io_channel_unix_new(sk);
+	g_io_add_watch(sk_io, G_IO_IN | G_IO_NVAL, io_conn_ind, NULL);
+	vdev.scan = sk_io;
+	return 0;
+
+failed:
+	close(sk);
+	return 1;
+}
+
+static void accept_connection(uint8_t *data)
+{
+	accept_conn_req_cp *cp = (void *) data;
+	struct vhci_conn *conn;
+
+	if (!(conn = conn_get_by_bdaddr(&cp->bdaddr)))
+		return;
+
+	connect_complete(conn);
+
+	g_io_add_watch(conn->chan, G_IO_IN | G_IO_NVAL | G_IO_HUP,
+			io_acl_data, (gpointer) conn);
+}
+
+static void close_connection(struct vhci_conn *conn)
+{
+	char addr[18];
+
+	ba2str(&conn->dest, addr);
+	syslog(LOG_INFO, "Closing connection %s handle %d",
+					addr, conn->handle);
+
+	g_io_channel_shutdown(conn->chan, TRUE, NULL);
+	g_io_channel_unref(conn->chan);
+
+	vconn[conn->handle - 1] = NULL;
+	disconn_complete(conn);
+	free(conn);
+}
+
+static void disconnect(uint8_t *data)
+{
+	disconnect_cp *cp = (void *) data;
+	struct vhci_conn *conn;
+	uint16_t handle;
+
+	handle = btohs(cp->handle);
+
+	if (handle > VHCI_MAX_CONN)
+		return;
+
+	if (!(conn = vconn[handle-1]))
+		return;
+
+	close_connection(conn);
+}
+
+static void create_connection(uint8_t *data)
+{
+	create_conn_cp *cp = (void *) data;
+	struct vhci_link_info info;
+	struct vhci_conn *conn;
+	struct sockaddr_in sa;
+	int h, sk, opt;
+	bdaddr_t ba;
+
+	for (h = 0; h < VHCI_MAX_CONN; h++)
+		if (!vconn[h])
+			goto do_connect;
+
+	syslog(LOG_ERR, "Too many connections");
+	return;
+
+do_connect:
+	if ((sk = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+		syslog(LOG_ERR, "Can't create socket: %s (%d)",
+						strerror(errno), errno);
+		return;
+	}
+
+	opt = 1;
+	setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+
+	baswap(&ba, &vdev.bdaddr);
+	sa.sin_family = AF_INET;
+	sa.sin_addr.s_addr = INADDR_ANY;	// *(uint32_t *) &ba;
+	sa.sin_port = 0;			// *(uint16_t *) &ba.b[4];
+	if (bind(sk, (struct sockaddr *) &sa, sizeof(sa))) {
+		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
+						strerror(errno), errno);
+		close(sk);
+		return;
+	}
+
+	baswap(&ba, &cp->bdaddr);
+	sa.sin_family = AF_INET;
+	memcpy(&sa.sin_addr.s_addr, &ba, sizeof(sa.sin_addr.s_addr));
+	sa.sin_port = *(uint16_t *) &ba.b[4];
+	if (connect(sk, (struct sockaddr *) &sa, sizeof(sa)) < 0) {
+		syslog(LOG_ERR, "Can't connect: %s (%d)",
+						strerror(errno), errno);
+		close(sk);
+		return;
+	}
+
+	/* Send info */
+	memset(&info, 0, sizeof(info));
+	bacpy(&info.bdaddr, &vdev.bdaddr);
+	info.link_type = ACL_LINK;
+	info.role = 1;
+	write_n(sk, (void *) &info, sizeof(info));
+
+	if (!(conn = malloc(sizeof(*conn)))) {
+		syslog(LOG_ERR, "Can't alloc new connection: %s (%d)",
+						strerror(errno), errno);
+		close(sk);
+		return;
+	}
+
+	memcpy((uint8_t *) &ba, (uint8_t *) &sa.sin_addr, 4);
+	memcpy((uint8_t *) &ba.b[4], (uint8_t *) &sa.sin_port, 2);
+	baswap(&conn->dest, &ba);
+
+	vconn[h] = conn;
+	conn->handle = h + 1;
+	conn->chan = g_io_channel_unix_new(sk);
+
+	connect_complete(conn);
+	g_io_add_watch(conn->chan, G_IO_IN | G_IO_NVAL | G_IO_HUP,
+				io_acl_data, (gpointer) conn);
+	return;
+}
+
+static void hci_link_control(uint16_t ocf, int plen, uint8_t *data)
+{
+	uint8_t status;
+
+	const uint16_t ogf = OGF_LINK_CTL;
+
+	switch (ocf) {
+	case OCF_CREATE_CONN:
+		command_status(ogf, ocf, 0x00);
+		create_connection(data);
+		break;
+
+	case OCF_ACCEPT_CONN_REQ:
+		command_status(ogf, ocf, 0x00);
+		accept_connection(data);
+		break;
+
+	case OCF_DISCONNECT:
+		command_status(ogf, ocf, 0x00);
+		disconnect(data);
+		break;
+
+	default:
+		status = 0x01;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+	}
+}
+
+static void hci_link_policy(uint16_t ocf, int plen, uint8_t *data)
+{
+	uint8_t status;
+
+	const uint16_t ogf = OGF_INFO_PARAM;
+
+	switch (ocf) {
+	default:
+		status = 0x01;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+	}
+}
+
+static void hci_host_control(uint16_t ocf, int plen, uint8_t *data)
+{
+	read_local_name_rp ln;
+	read_class_of_dev_rp cd;
+	read_inquiry_mode_rp im;
+	read_ext_inquiry_response_rp ir;
+	uint8_t status;
+
+	const uint16_t ogf = OGF_HOST_CTL;
+
+	switch (ocf) {
+	case OCF_RESET:
+		status = 0x00;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_SET_EVENT_FLT:
+		status = 0x00;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_CHANGE_LOCAL_NAME:
+		status = 0x00;
+		memcpy(vdev.name, data, sizeof(vdev.name));
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_READ_LOCAL_NAME:
+		ln.status = 0x00;
+		memcpy(ln.name, vdev.name, sizeof(ln.name));
+		command_complete(ogf, ocf, sizeof(ln), &ln);
+		break;
+
+	case OCF_WRITE_CONN_ACCEPT_TIMEOUT:
+	case OCF_WRITE_PAGE_TIMEOUT:
+		status = 0x00;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_WRITE_SCAN_ENABLE:
+		status = scan_enable(data);
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_WRITE_AUTH_ENABLE:
+		status = 0x00;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_WRITE_ENCRYPT_MODE:
+		status = 0x00;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_READ_CLASS_OF_DEV:
+		cd.status = 0x00;
+		memcpy(cd.dev_class, vdev.dev_class, 3);
+		command_complete(ogf, ocf, sizeof(cd), &cd);
+		break;
+
+	case OCF_WRITE_CLASS_OF_DEV:
+		status = 0x00;
+		memcpy(vdev.dev_class, data, 3);
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_READ_INQUIRY_MODE:
+		im.status = 0x00;
+		im.mode = vdev.inq_mode;
+		command_complete(ogf, ocf, sizeof(im), &im);
+		break;
+
+	case OCF_WRITE_INQUIRY_MODE:
+		status = 0x00;
+		vdev.inq_mode = data[0];
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	case OCF_READ_EXT_INQUIRY_RESPONSE:
+		ir.status = 0x00;
+		ir.fec = vdev.eir_fec;
+		memcpy(ir.data, vdev.eir_data, 240);
+		command_complete(ogf, ocf, sizeof(ir), &ir);
+		break;
+
+	case OCF_WRITE_EXT_INQUIRY_RESPONSE:
+		status = 0x00;
+		vdev.eir_fec = data[0];
+		memcpy(vdev.eir_data, data + 1, 240);
+		command_complete(ogf, ocf, 1, &status);
+		break;
+
+	default:
+		status = 0x01;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+	}
+}
+
+static void hci_info_param(uint16_t ocf, int plen, uint8_t *data)
+{
+	read_local_version_rp lv;
+	read_local_features_rp lf;
+	read_local_ext_features_rp ef;
+	read_buffer_size_rp bs;
+	read_bd_addr_rp ba;
+	uint8_t status;
+
+	const uint16_t ogf = OGF_INFO_PARAM;
+
+	switch (ocf) {
+	case OCF_READ_LOCAL_VERSION:
+		lv.status = 0x00;
+		lv.hci_ver = 0x03;
+		lv.hci_rev = htobs(0x0000);
+		lv.lmp_ver = 0x03;
+		lv.manufacturer = htobs(29);
+		lv.lmp_subver = htobs(0x0000);
+		command_complete(ogf, ocf, sizeof(lv), &lv);
+		break;
+
+	case OCF_READ_LOCAL_FEATURES:
+		lf.status = 0x00;
+		memcpy(lf.features, vdev.features, 8);
+		command_complete(ogf, ocf, sizeof(lf), &lf);
+		break;
+
+	case OCF_READ_LOCAL_EXT_FEATURES:
+		ef.status = 0x00;
+		if (*data == 0) {
+			ef.page_num = 0;
+			ef.max_page_num = 0;
+			memcpy(ef.features, vdev.features, 8);
+		} else {
+			ef.page_num = *data;
+			ef.max_page_num = 0;
+			memset(ef.features, 0, 8);
+		}
+		command_complete(ogf, ocf, sizeof(ef), &ef);
+		break;
+
+	case OCF_READ_BUFFER_SIZE:
+		bs.status = 0x00;
+		bs.acl_mtu = htobs(VHCI_ACL_MTU);
+		bs.sco_mtu = 0;
+		bs.acl_max_pkt = htobs(VHCI_ACL_MAX_PKT);
+		bs.sco_max_pkt = htobs(0);
+		command_complete(ogf, ocf, sizeof(bs), &bs);
+		break;
+
+	case OCF_READ_BD_ADDR:
+		ba.status = 0x00;
+		bacpy(&ba.bdaddr, &vdev.bdaddr);
+		command_complete(ogf, ocf, sizeof(ba), &ba);
+		break;
+
+	default:
+		status = 0x01;
+		command_complete(ogf, ocf, 1, &status);
+		break;
+	}
+}
+
+static void hci_command(uint8_t *data)
+{
+	hci_command_hdr *ch;
+	uint8_t *ptr = data;
+	uint16_t ogf, ocf;
+
+	ch = (hci_command_hdr *) ptr;
+	ptr += HCI_COMMAND_HDR_SIZE;
+
+	ch->opcode = btohs(ch->opcode);
+	ogf = cmd_opcode_ogf(ch->opcode);
+	ocf = cmd_opcode_ocf(ch->opcode);
+
+	switch (ogf) {
+	case OGF_LINK_CTL:
+		hci_link_control(ocf, ch->plen, ptr);
+		break;
+
+	case OGF_LINK_POLICY:
+		hci_link_policy(ocf, ch->plen, ptr);
+		break;
+
+	case OGF_HOST_CTL:
+		hci_host_control(ocf, ch->plen, ptr);
+		break;
+
+	case OGF_INFO_PARAM:
+		hci_info_param(ocf, ch->plen, ptr);
+		break;
+	}
+}
+
+static void hci_acl_data(uint8_t *data)
+{
+	hci_acl_hdr *ah = (void *) data;
+	struct vhci_conn *conn;
+	uint16_t handle;
+	int fd;
+
+	handle = acl_handle(btohs(ah->handle));
+
+	if (handle > VHCI_MAX_CONN || !(conn = vconn[handle - 1])) {
+		syslog(LOG_ERR, "Bad connection handle %d", handle);
+		return;
+	}
+
+	fd = g_io_channel_unix_get_fd(conn->chan);
+	if (write_n(fd, data, btohs(ah->dlen) + HCI_ACL_HDR_SIZE) < 0) {
+		close_connection(conn);
+		return;
+	}
+
+	if (++vdev.acl_cnt > VHCI_ACL_MAX_PKT - 1) {
+		/* Send num of complete packets event */
+		num_completed_pkts(conn);
+		vdev.acl_cnt = 0;
+	}
+}
+
+static gboolean io_acl_data(GIOChannel *chan, GIOCondition cond, gpointer data)
+{
+	struct vhci_conn *conn = (struct vhci_conn *) data;
+	unsigned char buf[HCI_MAX_FRAME_SIZE], *ptr;
+	hci_acl_hdr *ah;
+	uint16_t flags;
+	int fd, err, len;
+
+	if (cond & G_IO_NVAL) {
+		g_io_channel_unref(chan);
+		return FALSE;
+	}
+
+	if (cond & G_IO_HUP) {
+		close_connection(conn);
+		return FALSE;
+	}
+
+	fd = g_io_channel_unix_get_fd(chan);
+
+	ptr = buf + 1;
+	if (read_n(fd, ptr, HCI_ACL_HDR_SIZE) <= 0) {
+		close_connection(conn);
+		return FALSE;
+	}
+
+	ah = (void *) ptr;
+	ptr += HCI_ACL_HDR_SIZE;
+
+	len = btohs(ah->dlen);
+	if (read_n(fd, ptr, len) <= 0) {
+		close_connection(conn);
+		return FALSE;
+	}
+
+	buf[0] = HCI_ACLDATA_PKT;
+
+	flags = acl_flags(btohs(ah->handle));
+	ah->handle = htobs(acl_handle_pack(conn->handle, flags));
+	len += HCI_ACL_HDR_SIZE + 1;
+
+	write_snoop(vdev.dd, HCI_ACLDATA_PKT, 1, buf, len);
+
+	err = write(vdev.fd, buf, len);
+
+	return TRUE;
+}
+
+static gboolean io_conn_ind(GIOChannel *chan, GIOCondition cond, gpointer data)
+{
+	struct vhci_link_info info;
+	struct vhci_conn *conn;
+	struct sockaddr_in sa;
+	socklen_t len;
+	int sk, nsk, h;
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	sk = g_io_channel_unix_get_fd(chan);
+
+	len = sizeof(sa);
+	if ((nsk = accept(sk, (struct sockaddr *) &sa, &len)) < 0)
+		return TRUE;
+
+	if (read_n(nsk, &info, sizeof(info)) < 0) {
+		syslog(LOG_ERR, "Can't read link info");
+		return TRUE;
+	}
+
+	if (!(conn = malloc(sizeof(*conn)))) {
+		syslog(LOG_ERR, "Can't alloc new connection");
+		close(nsk);
+		return TRUE;
+	}
+
+	bacpy(&conn->dest, &info.bdaddr);
+
+	for (h = 0; h < VHCI_MAX_CONN; h++)
+		if (!vconn[h])
+			goto accepted;
+
+	syslog(LOG_ERR, "Too many connections");
+	free(conn);
+	close(nsk);
+	return TRUE;
+
+accepted:
+	vconn[h] = conn;
+	conn->handle = h + 1;
+	conn->chan = g_io_channel_unix_new(nsk);
+	connect_request(conn);
+
+	return TRUE;
+}
+
+static gboolean io_hci_data(GIOChannel *chan, GIOCondition cond, gpointer data)
+{
+	unsigned char buf[HCI_MAX_FRAME_SIZE], *ptr;
+	int type;
+	ssize_t len;
+	int fd;
+
+	ptr = buf;
+
+	fd = g_io_channel_unix_get_fd(chan);
+
+	len = read(fd, buf, sizeof(buf));
+	if (len < 0) {
+		if (errno == EAGAIN)
+			return TRUE;
+
+		syslog(LOG_ERR, "Read failed: %s (%d)", strerror(errno), errno);
+		g_io_channel_unref(chan);
+		g_main_loop_quit(event_loop);
+		return FALSE;
+	}
+
+	type = *ptr++;
+
+	write_snoop(vdev.dd, type, 0, buf, len);
+
+	switch (type) {
+	case HCI_COMMAND_PKT:
+		hci_command(ptr);
+		break;
+
+	case HCI_ACLDATA_PKT:
+		hci_acl_data(ptr);
+		break;
+
+	default:
+		syslog(LOG_ERR, "Unknown packet type 0x%2.2x", type);
+		break;
+	}
+
+	return TRUE;
+}
+
+static int getbdaddrbyname(char *str, bdaddr_t *ba)
+{
+	int i, n, len;
+
+	len = strlen(str);
+
+	/* Check address format */
+	for (i = 0, n = 0; i < len; i++)
+		if (str[i] == ':')
+			n++;
+
+	if (n == 5) {
+		/* BD address */
+		str2ba(str, ba);
+		return 0;
+	}
+
+	if (n == 1) {
+		/* IP address + port */
+		struct hostent *hent;
+		bdaddr_t b;
+		char *ptr;
+
+		ptr = strchr(str, ':');
+		*ptr++ = 0;
+
+		if (!(hent = gethostbyname(str))) {
+			fprintf(stderr, "Can't resolve %s\n", str);
+			return -2;
+		}
+
+		memcpy(&b, hent->h_addr, 4);
+		*(uint16_t *) (&b.b[4]) = htons(atoi(ptr));
+		baswap(ba, &b);
+
+		return 0;
+	}
+
+	fprintf(stderr, "Invalid address format\n");
+
+	return -1;
+}
+
+static void rewrite_bdaddr(unsigned char *buf, int len, bdaddr_t *bdaddr)
+{
+	hci_event_hdr *eh;
+	unsigned char *ptr = buf;
+	int type;
+
+	if (!bdaddr)
+		return;
+
+	if (!bacmp(bdaddr, BDADDR_ANY))
+		return;
+
+	type = *ptr++;
+
+	switch (type) {
+	case HCI_EVENT_PKT:
+		eh = (hci_event_hdr *) ptr;
+		ptr += HCI_EVENT_HDR_SIZE;
+
+		if (eh->evt == EVT_CMD_COMPLETE) {
+			evt_cmd_complete *cc = (void *) ptr;
+
+			ptr += EVT_CMD_COMPLETE_SIZE;
+
+			if (cc->opcode == htobs(cmd_opcode_pack(OGF_INFO_PARAM,
+						OCF_READ_BD_ADDR))) {
+				bacpy((bdaddr_t *) (ptr + 1), bdaddr);
+			}
+		}
+		break;
+	}
+}
+
+static int run_proxy(int fd, int dev, bdaddr_t *bdaddr)
+{
+	unsigned char buf[HCI_MAX_FRAME_SIZE + 1];
+	struct hci_dev_info di;
+	struct hci_filter flt;
+	struct pollfd p[2];
+	int dd, err, len, need_raw;
+
+	dd = hci_open_dev(dev);
+	if (dd < 0) {
+		syslog(LOG_ERR, "Can't open device hci%d: %s (%d)",
+						dev, strerror(errno), errno);
+		return 1;
+	}
+
+	if (hci_devinfo(dev, &di) < 0) {
+		syslog(LOG_ERR, "Can't get device info for hci%d: %s (%d)",
+						dev, strerror(errno), errno);
+		hci_close_dev(dd);
+		return 1;
+	}
+
+	need_raw = !hci_test_bit(HCI_RAW, &di.flags);
+
+	hci_filter_clear(&flt);
+	hci_filter_all_ptypes(&flt);
+	hci_filter_all_events(&flt);
+
+	if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
+		syslog(LOG_ERR, "Can't set filter for hci%d: %s (%d)",
+						dev, strerror(errno), errno);
+		hci_close_dev(dd);
+		return 1;
+	}
+
+	if (need_raw) {
+		if (ioctl(dd, HCISETRAW, 1) < 0) {
+			syslog(LOG_ERR, "Can't set raw mode on hci%d: %s (%d)",
+						dev, strerror(errno), errno);
+			hci_close_dev(dd);
+			return 1;
+		}
+	}
+
+	p[0].fd = fd;
+	p[0].events = POLLIN;
+	p[1].fd = dd;
+	p[1].events = POLLIN;
+
+	while (!__io_canceled) {
+		p[0].revents = 0;
+		p[1].revents = 0;
+		err = poll(p, 2, 500);
+		if (err < 0)
+			break;
+		if (!err)
+			continue;
+
+		if (p[0].revents & POLLIN) {
+			len = read(fd, buf, sizeof(buf));
+			if (len > 0) {
+				rewrite_bdaddr(buf, len, bdaddr);
+				err = write(dd, buf, len);
+			}
+		}
+
+		if (p[1].revents & POLLIN) {
+			len = read(dd, buf, sizeof(buf));
+			if (len > 0) {
+				rewrite_bdaddr(buf, len, bdaddr);
+				err = write(fd, buf, len);
+			}
+		}
+	}
+
+	if (need_raw) {
+		if (ioctl(dd, HCISETRAW, 0) < 0)
+			syslog(LOG_ERR, "Can't clear raw mode on hci%d: %s (%d)",
+						dev, strerror(errno), errno);
+	}
+
+	hci_close_dev(dd);
+
+	syslog(LOG_INFO, "Exit");
+
+	return 0;
+}
+
+static void usage(void)
+{
+	printf("hciemu - HCI emulator ver %s\n", VERSION);
+	printf("Usage: \n");
+	printf("\thciemu [options] local_address\n"
+		"Options:\n"
+		"\t[-d device] use specified device\n"
+		"\t[-b bdaddr] emulate specified address\n"
+		"\t[-s file] create snoop file\n"
+		"\t[-n] do not detach\n"
+		"\t[-h] help, you are looking at it\n");
+}
+
+static struct option main_options[] = {
+	{ "device",	1, 0, 'd' },
+	{ "bdaddr",	1, 0, 'b' },
+	{ "snoop",	1, 0, 's' },
+	{ "nodetach",	0, 0, 'n' },
+	{ "help",	0, 0, 'h' },
+	{ 0 }
+};
+
+int main(int argc, char *argv[])
+{
+	struct sigaction sa;
+	GIOChannel *dev_io;
+	char *device = NULL, *snoop = NULL;
+	bdaddr_t bdaddr;
+	int fd, dd, opt, detach = 1, dev = -1;
+
+	bacpy(&bdaddr, BDADDR_ANY);
+
+	while ((opt=getopt_long(argc, argv, "d:b:s:nh", main_options, NULL)) != EOF) {
+		switch(opt) {
+		case 'd':
+			device = strdup(optarg);
+			break;
+
+		case 'b':
+			str2ba(optarg, &bdaddr);
+			break;
+
+		case 's':
+			snoop = strdup(optarg);
+			break;
+
+		case 'n':
+			detach = 0;
+			break;
+
+		case 'h':
+		default:
+			usage();
+			exit(0);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+	optind = 0;
+
+	if (argc < 1) {
+		usage();
+		exit(1);
+	}
+
+	if (strlen(argv[0]) > 3 && !strncasecmp(argv[0], "hci", 3)) {
+		dev = hci_devid(argv[0]);
+		if (dev < 0) {
+			perror("Invalid device");
+			exit(1);
+		}
+	} else {
+		if (getbdaddrbyname(argv[0], &vdev.bdaddr) < 0)
+			exit(1);
+	}
+
+	if (detach) {
+		if (daemon(0, 0)) {
+			perror("Can't start daemon");
+			exit(1);
+		}
+	}
+
+	/* Start logging to syslog and stderr */
+	openlog("hciemu", LOG_PID | LOG_NDELAY | LOG_PERROR, LOG_DAEMON);
+	syslog(LOG_INFO, "HCI emulation daemon ver %s started", VERSION);
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_flags   = SA_NOCLDSTOP;
+	sa.sa_handler = SIG_IGN;
+	sigaction(SIGCHLD, &sa, NULL);
+	sigaction(SIGPIPE, &sa, NULL);
+
+	sa.sa_handler = sig_term;
+	sigaction(SIGTERM, &sa, NULL);
+	sigaction(SIGINT,  &sa, NULL);
+
+	io_init();
+
+	if (!device && dev >= 0)
+		device = strdup(GHCI_DEV);
+
+	/* Open and create virtual HCI device */
+	if (device) {
+		fd = open(device, O_RDWR);
+		if (fd < 0) {
+			syslog(LOG_ERR, "Can't open device %s: %s (%d)",
+						device, strerror(errno), errno);
+			free(device);
+			exit(1);
+		}
+		free(device);
+	} else {
+		fd = open(VHCI_DEV, O_RDWR);
+		if (fd < 0) {
+			fd = open(VHCI_UDEV, O_RDWR);
+			if (fd < 0) {
+				syslog(LOG_ERR, "Can't open device %s: %s (%d)",
+						VHCI_DEV, strerror(errno), errno);
+				exit(1);
+			}
+		}
+	}
+
+	/* Create snoop file */
+	if (snoop) {
+		dd = create_snoop(snoop);
+		if (dd < 0)
+			syslog(LOG_ERR, "Can't create snoop file %s: %s (%d)",
+						snoop, strerror(errno), errno);
+		free(snoop);
+	} else
+		dd = -1;
+
+	/* Create event loop */
+	event_loop = g_main_loop_new(NULL, FALSE);
+
+	if (dev >= 0)
+		return run_proxy(fd, dev, &bdaddr);
+
+	/* Device settings */
+	vdev.features[0] = 0xff;
+	vdev.features[1] = 0xff;
+	vdev.features[2] = 0x8f;
+	vdev.features[3] = 0xfe;
+	vdev.features[4] = 0x9b;
+	vdev.features[5] = 0xf9;
+	vdev.features[6] = 0x01;
+	vdev.features[7] = 0x80;
+
+	memset(vdev.name, 0, sizeof(vdev.name));
+	strncpy((char *) vdev.name, "BlueZ (Virtual HCI)",
+							sizeof(vdev.name) - 1);
+
+	vdev.dev_class[0] = 0x00;
+	vdev.dev_class[1] = 0x00;
+	vdev.dev_class[2] = 0x00;
+
+	vdev.inq_mode = 0x00;
+	vdev.eir_fec = 0x00;
+	memset(vdev.eir_data, 0, sizeof(vdev.eir_data));
+
+	vdev.fd = fd;
+	vdev.dd = dd;
+
+	dev_io = g_io_channel_unix_new(fd);
+	g_io_add_watch(dev_io, G_IO_IN, io_hci_data, NULL);
+
+	setpriority(PRIO_PROCESS, 0, -19);
+
+	/* Start event processor */
+	g_main_loop_run(event_loop);
+
+	close(fd);
+
+	if (dd >= 0)
+		close(dd);
+
+	syslog(LOG_INFO, "Exit");
+
+	return 0;
+}
diff --git a/test/hsmicro b/test/hsmicro
new file mode 100755
index 0000000..c254226
--- /dev/null
+++ b/test/hsmicro
@@ -0,0 +1,20 @@
+#!/bin/sh
+
+SOX=`which sox`
+HSTEST=`which hstest`
+
+if [ -z "$HSTEST" ]
+then
+	HSTEST="./hstest"
+fi
+
+if [ -z "$1" ]
+then
+	echo -e "Usage:\n\thsmicro <bdaddr> [channel]"
+	exit
+fi
+
+BDADDR=$1
+CHANNEL=$2
+
+$HSTEST record - $BDADDR $CHANNEL | $SOX -t raw -r 8000 -c 1 -s -w - -t ossdsp -r 44100 -c 2 -s -w /dev/dsp polyphase vol 5.0 2> /dev/null
diff --git a/test/hsplay b/test/hsplay
new file mode 100755
index 0000000..8cecbff
--- /dev/null
+++ b/test/hsplay
@@ -0,0 +1,22 @@
+#!/bin/sh
+
+MPG123=`which mpg123`
+SOX=`which sox`
+HSTEST=`which hstest`
+
+if [ -z "$HSTEST" ]
+then
+	HSTEST="./hstest"
+fi
+
+if [ -z "$1" ] || [ -z "$2" ]
+then
+	echo -e "Usage:\n\thsplay <file> <bdaddr> [channel]"
+	exit
+fi
+
+FILE=$1
+BDADDR=$2
+CHANNEL=$3
+
+$MPG123 -q -s "$FILE" | $SOX -t raw -r 44100 -c 2 -s -w - -t raw -r 8000 -c 1 -s -w - | $HSTEST play - $BDADDR $CHANNEL
diff --git a/test/hstest.c b/test/hstest.c
new file mode 100644
index 0000000..08f2257
--- /dev/null
+++ b/test/hstest.c
@@ -0,0 +1,308 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <termios.h>
+#include <sys/wait.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/sco.h>
+#include <bluetooth/rfcomm.h>
+
+static volatile int terminate = 0;
+
+static void sig_term(int sig) {
+	terminate = 1;
+}
+
+static int rfcomm_connect(bdaddr_t *src, bdaddr_t *dst, uint8_t channel)
+{
+	struct sockaddr_rc addr;
+	int s;
+
+	if ((s = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM)) < 0) {
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.rc_family = AF_BLUETOOTH;
+	bacpy(&addr.rc_bdaddr, src);
+	addr.rc_channel = 0;
+	if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+		close(s);
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.rc_family = AF_BLUETOOTH;
+	bacpy(&addr.rc_bdaddr, dst);
+	addr.rc_channel = channel;
+	if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) < 0 ){
+		close(s);
+		return -1;
+	}
+
+	return s;
+}
+
+static int sco_connect(bdaddr_t *src, bdaddr_t *dst, uint16_t *handle, uint16_t *mtu)
+{
+	struct sockaddr_sco addr;
+	struct sco_conninfo conn;
+	struct sco_options opts;
+	socklen_t size;
+	int s;
+
+	if ((s = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO)) < 0) {
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sco_family = AF_BLUETOOTH;
+	bacpy(&addr.sco_bdaddr, src);
+
+	if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+		close(s);
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sco_family = AF_BLUETOOTH;
+	bacpy(&addr.sco_bdaddr, dst);
+
+	if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) < 0 ){
+		close(s);
+		return -1;
+	}
+
+	memset(&conn, 0, sizeof(conn));
+	size = sizeof(conn);
+
+	if (getsockopt(s, SOL_SCO, SCO_CONNINFO, &conn, &size) < 0) {
+		close(s);
+		return -1;
+	}
+
+	memset(&opts, 0, sizeof(opts));
+	size = sizeof(opts);
+
+	if (getsockopt(s, SOL_SCO, SCO_OPTIONS, &opts, &size) < 0) {
+		close(s);
+		return -1;
+	}
+
+	if (handle)
+		*handle = conn.hci_handle;
+
+	if (mtu)
+		*mtu = opts.mtu;
+
+	return s;
+}
+
+static void usage(void)
+{
+	printf("Usage:\n"
+		"\thstest play   <file> <bdaddr> [channel]\n"
+		"\thstest record <file> <bdaddr> [channel]\n");
+}
+
+#define PLAY	1
+#define RECORD	2
+
+int main(int argc, char *argv[])
+{
+	struct sigaction sa;
+
+	fd_set rfds;
+	struct timeval timeout;
+	unsigned char buf[2048], *p;
+	int maxfd, sel, rlen, wlen;
+
+	bdaddr_t local;
+	bdaddr_t bdaddr;
+	uint8_t channel;
+
+	char *filename;
+	mode_t filemode;
+	int err, mode = 0;
+	int dd, rd, sd, fd;
+	uint16_t sco_handle, sco_mtu, vs;
+
+	switch (argc) {
+	case 4:
+		str2ba(argv[3], &bdaddr);
+		channel = 6;
+		break;
+	case 5:
+		str2ba(argv[3], &bdaddr);
+		channel = atoi(argv[4]);
+		break;
+	default:
+		usage();
+		exit(-1);
+	}
+
+	if (strncmp(argv[1], "play", 4) == 0) {
+		mode = PLAY;
+		filemode = O_RDONLY;
+	} else if (strncmp(argv[1], "rec", 3) == 0) {
+		mode = RECORD;
+		filemode = O_WRONLY | O_CREAT | O_TRUNC;
+	} else {
+		usage();
+		exit(-1);
+	}
+
+	filename = argv[2];
+
+	hci_devba(0, &local);
+	dd = hci_open_dev(0);
+	hci_read_voice_setting(dd, &vs, 1000);
+	vs = htobs(vs);
+	fprintf(stderr, "Voice setting: 0x%04x\n", vs);
+	close(dd);
+	if (vs != 0x0060) {
+		fprintf(stderr, "The voice setting must be 0x0060\n");
+		return -1;
+	}
+
+	if (strcmp(filename, "-") == 0) {
+		switch (mode) {
+		case PLAY:
+			fd = 0;
+			break;
+		case RECORD:
+			fd = 1;
+			break;
+		default:
+			return -1;
+		}
+	} else {
+		if ((fd = open(filename, filemode)) < 0) {
+			perror("Can't open input/output file");
+			return -1;
+		}
+	}
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_flags = SA_NOCLDSTOP;
+	sa.sa_handler = sig_term;
+	sigaction(SIGTERM, &sa, NULL);
+	sigaction(SIGINT,  &sa, NULL);
+
+	sa.sa_handler = SIG_IGN;
+	sigaction(SIGCHLD, &sa, NULL);
+	sigaction(SIGPIPE, &sa, NULL);
+
+	if ((rd = rfcomm_connect(&local, &bdaddr, channel)) < 0) {
+		perror("Can't connect RFCOMM channel");
+		return -1;
+	}
+
+	fprintf(stderr, "RFCOMM channel connected\n");
+
+	if ((sd = sco_connect(&local, &bdaddr, &sco_handle, &sco_mtu)) < 0) {
+		perror("Can't connect SCO audio channel");
+		close(rd);
+		return -1;
+	}
+
+	fprintf(stderr, "SCO audio channel connected (handle %d, mtu %d)\n", sco_handle, sco_mtu);
+
+	if (mode == RECORD)
+		err = write(rd, "RING\r\n", 6);
+
+	maxfd = (rd > sd) ? rd : sd;
+
+	while (!terminate) {
+
+		FD_ZERO(&rfds);
+		FD_SET(rd, &rfds);
+		FD_SET(sd, &rfds);
+
+		timeout.tv_sec = 0;
+		timeout.tv_usec = 10000;
+
+		if ((sel = select(maxfd + 1, &rfds, NULL, NULL, &timeout)) > 0) {
+
+			if (FD_ISSET(rd, &rfds)) {
+				memset(buf, 0, sizeof(buf));
+				rlen = read(rd, buf, sizeof(buf));
+				if (rlen > 0) {
+					fprintf(stderr, "%s\n", buf);
+					wlen = write(rd, "OK\r\n", 4);
+				}
+			}
+
+			if (FD_ISSET(sd, &rfds)) {
+				memset(buf, 0, sizeof(buf));
+				rlen = read(sd, buf, sizeof(buf));
+				if (rlen > 0)
+					switch (mode) {
+					case PLAY:
+						rlen = read(fd, buf, rlen);
+
+						wlen = 0;
+						p = buf;
+						while (rlen > sco_mtu) {
+						        wlen += write(sd, p, sco_mtu);
+						        rlen -= sco_mtu;
+						        p += sco_mtu;
+						}
+						wlen += write(sd, p, rlen);
+						break;
+					case RECORD:
+						wlen = write(fd, buf, rlen);
+						break;
+					default:
+						break;
+					}
+			}
+
+		}
+
+	}
+
+	close(sd);
+	sleep(5);
+	close(rd);
+
+	close(fd);
+
+	return 0;
+}
diff --git a/test/ipctest.c b/test/ipctest.c
new file mode 100644
index 0000000..9fdfac4
--- /dev/null
+++ b/test/ipctest.c
@@ -0,0 +1,1129 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2006-2010  Nokia Corporation
+ *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2009	Lennart Poettering
+ *  Copyright (C) 2008	Joao Paulo Rechi Vita
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library 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
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; 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 <stdlib.h>
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <assert.h>
+#include <libgen.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <signal.h>
+
+#include <glib.h>
+
+#include "ipc.h"
+#include "sbc.h"
+
+#define DBG(fmt, arg...)				\
+	printf("debug %s: " fmt "\n" , __FUNCTION__ , ## arg)
+#define ERR(fmt, arg...)				\
+	fprintf(stderr, "ERROR %s: " fmt "\n" , __FUNCTION__ , ## arg)
+
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
+
+#ifndef MIN
+# define MIN(x, y) ((x) < (y) ? (x) : (y))
+#endif
+
+#ifndef MAX
+# define MAX(x, y) ((x) > (y) ? (x) : (y))
+#endif
+
+#ifndef TRUE
+# define TRUE (1)
+#endif
+
+#ifndef FALSE
+# define FALSE (0)
+#endif
+
+#define YES_NO(t) ((t) ? "yes" : "no")
+
+#define BUFFER_SIZE 2048
+#define MAX_BITPOOL 64
+#define MIN_BITPOOL 2
+
+struct a2dp_info {
+	sbc_capabilities_t sbc_capabilities;
+	sbc_t sbc; /* Codec data */
+	int sbc_initialized; /* Keep track if the encoder is initialized */
+	size_t codesize; /* SBC codesize */
+
+	void* buffer; /* Codec transfer buffer */
+	size_t buffer_size; /* Size of the buffer */
+
+	uint16_t seq_num; /* Cumulative packet sequence */
+};
+
+struct hsp_info {
+	pcm_capabilities_t pcm_capabilities;
+};
+
+struct userdata {
+	int service_fd;
+	int stream_fd;
+	GIOChannel *stream_channel;
+	guint stream_watch;
+	GIOChannel *gin; /* dude, I am thirsty now */
+	guint gin_watch;
+	int transport;
+	uint32_t rate;
+	int channels;
+	char *address;
+	struct a2dp_info a2dp;
+	struct hsp_info hsp;
+	size_t link_mtu;
+	size_t block_size;
+	gboolean debug_stream_read : 1;
+	gboolean debug_stream_write : 1;
+};
+
+static struct userdata data = {
+	.service_fd = -1,
+	.stream_fd = -1,
+	.transport = BT_CAPABILITIES_TRANSPORT_A2DP,
+	.rate = 48000,
+	.channels = 2,
+	.address = NULL
+};
+
+static int start_stream(struct userdata *u);
+static int stop_stream(struct userdata *u);
+static gboolean input_cb(GIOChannel *gin, GIOCondition condition, gpointer data);
+
+static GMainLoop *main_loop;
+
+static int service_send(struct userdata *u, const bt_audio_msg_header_t *msg)
+{
+	int err;
+	uint16_t length;
+
+	assert(u);
+
+	length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE;
+
+	DBG("sending %s:%s", bt_audio_strtype(msg->type),
+		bt_audio_strname(msg->name));
+
+	if (send(u->service_fd, msg, length, 0) > 0)
+		err = 0;
+	else {
+		err = -errno;
+		ERR("Error sending data to audio service: %s(%d)",
+			strerror(errno), errno);
+	}
+
+	return err;
+}
+
+static int service_recv(struct userdata *u, bt_audio_msg_header_t *rsp)
+{
+	int err;
+	const char *type, *name;
+	uint16_t length;
+
+	assert(u);
+
+	length = rsp->length ? : BT_SUGGESTED_BUFFER_SIZE;
+
+	DBG("trying to receive msg from audio service...");
+	if (recv(u->service_fd, rsp, length, 0) > 0) {
+		type = bt_audio_strtype(rsp->type);
+		name = bt_audio_strname(rsp->name);
+		if (type && name) {
+			DBG("Received %s - %s", type, name);
+			err = 0;
+		} else {
+			err = -EINVAL;
+			ERR("Bogus message type %d - name %d"
+				"received from audio service",
+				rsp->type, rsp->name);
+		}
+	} else {
+		err = -errno;
+		ERR("Error receiving data from audio service: %s(%d)",
+			strerror(errno), errno);
+	}
+
+	return err;
+}
+
+static ssize_t service_expect(struct userdata *u, bt_audio_msg_header_t *rsp,
+				uint8_t expected_name)
+{
+	int r;
+
+	assert(u);
+	assert(u->service_fd >= 0);
+	assert(rsp);
+
+	if ((r = service_recv(u, rsp)) < 0)
+		return r;
+
+	if ((rsp->type != BT_INDICATION && rsp->type != BT_RESPONSE) ||
+			(rsp->name != expected_name)) {
+		if (rsp->type == BT_ERROR && rsp->length == sizeof(bt_audio_error_t))
+			ERR("Received error condition: %s",
+				strerror(((bt_audio_error_t*) rsp)->posix_errno));
+		else
+			ERR("Bogus message %s received while %s was expected",
+				bt_audio_strname(rsp->name),
+				bt_audio_strname(expected_name));
+		return -1;
+	}
+
+	return 0;
+}
+
+static int init_bt(struct userdata *u)
+{
+	assert(u);
+
+	if (u->service_fd != -1)
+		return 0;
+
+	DBG("bt_audio_service_open");
+
+	u->service_fd = bt_audio_service_open();
+	if (u->service_fd <= 0) {
+		perror(strerror(errno));
+		return errno;
+	}
+
+	return 0;
+}
+
+static int parse_caps(struct userdata *u, const struct bt_get_capabilities_rsp *rsp)
+{
+	unsigned char *ptr;
+	uint16_t bytes_left;
+	codec_capabilities_t codec;
+
+	assert(u);
+	assert(rsp);
+
+	bytes_left = rsp->h.length - sizeof(*rsp);
+
+	if (bytes_left < sizeof(codec_capabilities_t)) {
+		ERR("Packet too small to store codec information.");
+		return -1;
+	}
+
+	ptr = ((void *) rsp) + sizeof(*rsp);
+
+	memcpy(&codec, ptr, sizeof(codec)); /** ALIGNMENT? **/
+
+	DBG("Payload size is %lu %lu",
+		(unsigned long) bytes_left, (unsigned long) sizeof(codec));
+
+	if (u->transport != codec.transport) {
+		ERR("Got capabilities for wrong codec.");
+		return -1;
+	}
+
+	if (u->transport == BT_CAPABILITIES_TRANSPORT_SCO) {
+
+		if (bytes_left <= 0 ||
+				codec.length != sizeof(u->hsp.pcm_capabilities))
+			return -1;
+
+		assert(codec.type == BT_HFP_CODEC_PCM);
+
+		memcpy(&u->hsp.pcm_capabilities,
+				&codec, sizeof(u->hsp.pcm_capabilities));
+
+		DBG("Has NREC: %s",
+			YES_NO(u->hsp.pcm_capabilities.flags & BT_PCM_FLAG_NREC));
+
+	} else if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
+
+		while (bytes_left > 0) {
+			if (codec.type == BT_A2DP_SBC_SINK &&
+					!(codec.lock & BT_WRITE_LOCK))
+				break;
+
+			bytes_left -= codec.length;
+			ptr += codec.length;
+			memcpy(&codec, ptr, sizeof(codec));
+		}
+
+		DBG("bytes_left = %d, codec.length = %d",
+						bytes_left, codec.length);
+
+		if (bytes_left <= 0 ||
+				codec.length != sizeof(u->a2dp.sbc_capabilities))
+			return -1;
+
+		assert(codec.type == BT_A2DP_SBC_SINK);
+
+		memcpy(&u->a2dp.sbc_capabilities, &codec,
+					sizeof(u->a2dp.sbc_capabilities));
+	} else {
+		assert(0);
+	}
+
+	return 0;
+}
+
+static int get_caps(struct userdata *u)
+{
+	union {
+		struct bt_get_capabilities_req getcaps_req;
+		struct bt_get_capabilities_rsp getcaps_rsp;
+		bt_audio_error_t error;
+		uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
+	} msg;
+
+	assert(u);
+
+	memset(&msg, 0, sizeof(msg));
+	msg.getcaps_req.h.type = BT_REQUEST;
+	msg.getcaps_req.h.name = BT_GET_CAPABILITIES;
+	msg.getcaps_req.h.length = sizeof(msg.getcaps_req);
+
+	strncpy(msg.getcaps_req.destination, u->address,
+			sizeof(msg.getcaps_req.destination));
+	msg.getcaps_req.transport = u->transport;
+	msg.getcaps_req.flags = BT_FLAG_AUTOCONNECT;
+
+	if (service_send(u, &msg.getcaps_req.h) < 0)
+		return -1;
+
+	msg.getcaps_rsp.h.length = 0;
+	if (service_expect(u, &msg.getcaps_rsp.h, BT_GET_CAPABILITIES) < 0)
+		return -1;
+
+	return parse_caps(u, &msg.getcaps_rsp);
+}
+
+static uint8_t a2dp_default_bitpool(uint8_t freq, uint8_t mode)
+{
+	switch (freq) {
+	case BT_SBC_SAMPLING_FREQ_16000:
+	case BT_SBC_SAMPLING_FREQ_32000:
+		return 53;
+
+	case BT_SBC_SAMPLING_FREQ_44100:
+
+		switch (mode) {
+		case BT_A2DP_CHANNEL_MODE_MONO:
+		case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
+			return 31;
+
+		case BT_A2DP_CHANNEL_MODE_STEREO:
+		case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
+			return 53;
+
+		default:
+			DBG("Invalid channel mode %u", mode);
+			return 53;
+		}
+
+	case BT_SBC_SAMPLING_FREQ_48000:
+
+		switch (mode) {
+		case BT_A2DP_CHANNEL_MODE_MONO:
+		case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
+			return 29;
+
+		case BT_A2DP_CHANNEL_MODE_STEREO:
+		case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
+			return 51;
+
+		default:
+			DBG("Invalid channel mode %u", mode);
+			return 51;
+		}
+
+	default:
+		DBG("Invalid sampling freq %u", freq);
+		return 53;
+	}
+}
+
+static int setup_a2dp(struct userdata *u)
+{
+	sbc_capabilities_t *cap;
+	int i;
+
+	static const struct {
+		uint32_t rate;
+		uint8_t cap;
+	} freq_table[] = {
+		{ 16000U, BT_SBC_SAMPLING_FREQ_16000 },
+		{ 32000U, BT_SBC_SAMPLING_FREQ_32000 },
+		{ 44100U, BT_SBC_SAMPLING_FREQ_44100 },
+		{ 48000U, BT_SBC_SAMPLING_FREQ_48000 }
+	};
+
+	assert(u);
+	assert(u->transport == BT_CAPABILITIES_TRANSPORT_A2DP);
+
+	cap = &u->a2dp.sbc_capabilities;
+
+	/* Find the lowest freq that is at least as high as the requested
+	 * sampling rate */
+	for (i = 0; (unsigned) i < ARRAY_SIZE(freq_table); i++)
+		if (freq_table[i].rate >= u->rate &&
+			(cap->frequency & freq_table[i].cap)) {
+			u->rate = freq_table[i].rate;
+			cap->frequency = freq_table[i].cap;
+			break;
+		}
+
+	if ((unsigned) i >= ARRAY_SIZE(freq_table)) {
+		for (; i >= 0; i--) {
+			if (cap->frequency & freq_table[i].cap) {
+				u->rate = freq_table[i].rate;
+				cap->frequency = freq_table[i].cap;
+				break;
+			}
+		}
+
+		if (i < 0) {
+			DBG("Not suitable sample rate");
+			return -1;
+		}
+	}
+
+	if (u->channels <= 1) {
+		if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) {
+			cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
+			u->channels = 1;
+		} else
+			u->channels = 2;
+	}
+
+	if (u->channels >= 2) {
+		u->channels = 2;
+
+		if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
+			cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
+		else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
+			cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
+		else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
+			cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
+		else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) {
+			cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
+			u->channels = 1;
+		} else {
+			DBG("No supported channel modes");
+			return -1;
+		}
+	}
+
+	if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
+		cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
+	else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
+		cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
+	else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
+		cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
+	else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
+		cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
+	else {
+		DBG("No supported block lengths");
+		return -1;
+	}
+
+	if (cap->subbands & BT_A2DP_SUBBANDS_8)
+		cap->subbands = BT_A2DP_SUBBANDS_8;
+	else if (cap->subbands & BT_A2DP_SUBBANDS_4)
+		cap->subbands = BT_A2DP_SUBBANDS_4;
+	else {
+		DBG("No supported subbands");
+		return -1;
+	}
+
+	if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
+		cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
+	else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
+		cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
+
+	cap->min_bitpool = (uint8_t) MAX(MIN_BITPOOL, cap->min_bitpool);
+	cap->max_bitpool = (uint8_t) MIN(
+		a2dp_default_bitpool(cap->frequency, cap->channel_mode),
+		cap->max_bitpool);
+
+	return 0;
+}
+
+static void setup_sbc(struct a2dp_info *a2dp)
+{
+	sbc_capabilities_t *active_capabilities;
+
+	assert(a2dp);
+
+	active_capabilities = &a2dp->sbc_capabilities;
+
+	if (a2dp->sbc_initialized)
+		sbc_reinit(&a2dp->sbc, 0);
+	else
+		sbc_init(&a2dp->sbc, 0);
+	a2dp->sbc_initialized = TRUE;
+
+	switch (active_capabilities->frequency) {
+	case BT_SBC_SAMPLING_FREQ_16000:
+		a2dp->sbc.frequency = SBC_FREQ_16000;
+		break;
+	case BT_SBC_SAMPLING_FREQ_32000:
+		a2dp->sbc.frequency = SBC_FREQ_32000;
+		break;
+	case BT_SBC_SAMPLING_FREQ_44100:
+		a2dp->sbc.frequency = SBC_FREQ_44100;
+		break;
+	case BT_SBC_SAMPLING_FREQ_48000:
+		a2dp->sbc.frequency = SBC_FREQ_48000;
+		break;
+	default:
+		assert(0);
+	}
+
+	switch (active_capabilities->channel_mode) {
+	case BT_A2DP_CHANNEL_MODE_MONO:
+		a2dp->sbc.mode = SBC_MODE_MONO;
+		break;
+	case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
+		a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL;
+		break;
+	case BT_A2DP_CHANNEL_MODE_STEREO:
+		a2dp->sbc.mode = SBC_MODE_STEREO;
+		break;
+	case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
+		a2dp->sbc.mode = SBC_MODE_JOINT_STEREO;
+		break;
+	default:
+		assert(0);
+	}
+
+	switch (active_capabilities->allocation_method) {
+	case BT_A2DP_ALLOCATION_SNR:
+		a2dp->sbc.allocation = SBC_AM_SNR;
+		break;
+	case BT_A2DP_ALLOCATION_LOUDNESS:
+		a2dp->sbc.allocation = SBC_AM_LOUDNESS;
+		break;
+	default:
+		assert(0);
+	}
+
+	switch (active_capabilities->subbands) {
+	case BT_A2DP_SUBBANDS_4:
+		a2dp->sbc.subbands = SBC_SB_4;
+		break;
+	case BT_A2DP_SUBBANDS_8:
+		a2dp->sbc.subbands = SBC_SB_8;
+		break;
+	default:
+		assert(0);
+	}
+
+	switch (active_capabilities->block_length) {
+	case BT_A2DP_BLOCK_LENGTH_4:
+		a2dp->sbc.blocks = SBC_BLK_4;
+		break;
+	case BT_A2DP_BLOCK_LENGTH_8:
+		a2dp->sbc.blocks = SBC_BLK_8;
+		break;
+	case BT_A2DP_BLOCK_LENGTH_12:
+		a2dp->sbc.blocks = SBC_BLK_12;
+		break;
+	case BT_A2DP_BLOCK_LENGTH_16:
+		a2dp->sbc.blocks = SBC_BLK_16;
+		break;
+	default:
+		assert(0);
+	}
+
+	a2dp->sbc.bitpool = active_capabilities->max_bitpool;
+	a2dp->codesize = (uint16_t) sbc_get_codesize(&a2dp->sbc);
+}
+
+static int bt_open(struct userdata *u)
+{
+	union {
+		struct bt_open_req open_req;
+		struct bt_open_rsp open_rsp;
+		bt_audio_error_t error;
+		uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
+	} msg;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.open_req.h.type = BT_REQUEST;
+	msg.open_req.h.name = BT_OPEN;
+	msg.open_req.h.length = sizeof(msg.open_req);
+
+	strncpy(msg.open_req.destination, u->address,
+			sizeof(msg.open_req.destination));
+	msg.open_req.seid = u->transport == BT_CAPABILITIES_TRANSPORT_A2DP ?
+				u->a2dp.sbc_capabilities.capability.seid :
+				BT_A2DP_SEID_RANGE + 1;
+	msg.open_req.lock = u->transport == BT_CAPABILITIES_TRANSPORT_A2DP ?
+				BT_WRITE_LOCK : BT_READ_LOCK | BT_WRITE_LOCK;
+
+	if (service_send(u, &msg.open_req.h) < 0)
+		return -1;
+
+	msg.open_rsp.h.length = sizeof(msg.open_rsp);
+	if (service_expect(u, &msg.open_rsp.h, BT_OPEN) < 0)
+		return -1;
+
+	return 0;
+}
+
+static int set_conf(struct userdata *u)
+{
+	union {
+		struct bt_set_configuration_req setconf_req;
+		struct bt_set_configuration_rsp setconf_rsp;
+		bt_audio_error_t error;
+		uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
+	} msg;
+
+	if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
+		if (setup_a2dp(u) < 0)
+			return -1;
+	}
+
+	memset(&msg, 0, sizeof(msg));
+	msg.setconf_req.h.type = BT_REQUEST;
+	msg.setconf_req.h.name = BT_SET_CONFIGURATION;
+	msg.setconf_req.h.length = sizeof(msg.setconf_req);
+
+	if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
+		memcpy(&msg.setconf_req.codec, &u->a2dp.sbc_capabilities,
+			sizeof(u->a2dp.sbc_capabilities));
+		msg.setconf_req.h.length += msg.setconf_req.codec.length -
+			sizeof(msg.setconf_req.codec);
+	} else {
+		msg.setconf_req.codec.transport = BT_CAPABILITIES_TRANSPORT_SCO;
+		msg.setconf_req.codec.seid = BT_A2DP_SEID_RANGE + 1;
+		msg.setconf_req.codec.length = sizeof(pcm_capabilities_t);
+	}
+
+	if (service_send(u, &msg.setconf_req.h) < 0)
+		return -1;
+
+	msg.setconf_rsp.h.length = sizeof(msg.setconf_rsp);
+	if (service_expect(u, &msg.setconf_rsp.h, BT_SET_CONFIGURATION) < 0)
+		return -1;
+
+	u->link_mtu = msg.setconf_rsp.link_mtu;
+
+	/* setup SBC encoder now we agree on parameters */
+	if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
+		setup_sbc(&u->a2dp);
+		u->block_size = u->a2dp.codesize;
+		DBG("SBC parameters:\n\tallocation=%u\n"
+			"\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
+			u->a2dp.sbc.allocation, u->a2dp.sbc.subbands,
+			u->a2dp.sbc.blocks, u->a2dp.sbc.bitpool);
+	} else
+		u->block_size = u->link_mtu;
+
+	return 0;
+}
+
+static int setup_bt(struct userdata *u)
+{
+	assert(u);
+
+	if (get_caps(u) < 0)
+		return -1;
+
+	DBG("Got device caps");
+
+	if (bt_open(u) < 0)
+		return -1;
+
+	if (set_conf(u) < 0)
+		return -1;
+
+	return 0;
+}
+
+static int init_profile(struct userdata *u)
+{
+	assert(u);
+
+	return setup_bt(u);
+}
+
+static void shutdown_bt(struct userdata *u)
+{
+	assert(u);
+
+	if (u->stream_fd != -1) {
+		stop_stream(u);
+		DBG("close(stream_fd)");
+		close(u->stream_fd);
+		u->stream_fd = -1;
+	}
+
+	if (u->service_fd != -1) {
+		DBG("bt_audio_service_close");
+		bt_audio_service_close(u->service_fd);
+		u->service_fd = -1;
+	}
+}
+
+static void make_fd_nonblock(int fd)
+{
+	int v;
+
+	assert(fd >= 0);
+	assert((v = fcntl(fd, F_GETFL)) >= 0);
+
+	if (!(v & O_NONBLOCK))
+		assert(fcntl(fd, F_SETFL, v|O_NONBLOCK) >= 0);
+}
+
+static void make_socket_low_delay(int fd)
+{
+/* FIXME: is this widely supported? */
+#ifdef SO_PRIORITY
+	int priority;
+	assert(fd >= 0);
+
+	priority = 6;
+	if (setsockopt(fd, SOL_SOCKET, SO_PRIORITY, (void*)&priority,
+			sizeof(priority)) < 0)
+		ERR("SO_PRIORITY failed: %s", strerror(errno));
+#endif
+}
+
+static int read_stream(struct userdata *u)
+{
+	int ret = 0;
+	ssize_t l;
+	char *buf;
+
+	assert(u);
+	assert(u->stream_fd >= 0);
+
+	buf = alloca(u->link_mtu);
+
+	for (;;) {
+		l = read(u->stream_fd, buf, u->link_mtu);
+		if (u->debug_stream_read)
+			DBG("read from socket: %lli bytes", (long long) l);
+		if (l <= 0) {
+			if (l < 0 && errno == EINTR)
+				continue;
+			else {
+				ERR("Failed to read date from stream_fd: %s",
+					ret < 0 ? strerror(errno) : "EOF");
+				return -1;
+			}
+		} else {
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/* It's what PulseAudio is doing, not sure it's necessary for this
+ * test */
+static ssize_t pa_write(int fd, const void *buf, size_t count)
+{
+	ssize_t r;
+
+	if ((r = send(fd, buf, count, MSG_NOSIGNAL)) >= 0)
+		return r;
+
+	if (errno != ENOTSOCK)
+		return r;
+
+	return write(fd, buf, count);
+}
+
+static int write_stream(struct userdata *u)
+{
+	int ret = 0;
+	ssize_t l;
+	char *buf;
+
+	assert(u);
+	assert(u->stream_fd >= 0);
+	buf = alloca(u->link_mtu);
+
+	for (;;) {
+		l = pa_write(u->stream_fd, buf, u->link_mtu);
+		if (u->debug_stream_write)
+			DBG("written to socket: %lli bytes", (long long) l);
+		assert(l != 0);
+		if (l < 0) {
+			if (errno == EINTR)
+				continue;
+			else {
+				ERR("Failed to write data: %s", strerror(errno));
+				ret = -1;
+				break;
+			}
+		} else {
+			assert((size_t)l <= u->link_mtu);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static gboolean stream_cb(GIOChannel *gin, GIOCondition condition, gpointer data)
+{
+	struct userdata *u;
+
+	assert(u = data);
+
+	if (condition & G_IO_IN) {
+		if (read_stream(u) < 0)
+			goto fail;
+	} else if (condition & G_IO_OUT) {
+		if (write_stream(u) < 0)
+			goto fail;
+	} else {
+		DBG("Got %d", condition);
+		g_main_loop_quit(main_loop);
+		return FALSE;
+	}
+
+	return TRUE;
+
+fail:
+	stop_stream(u);
+	return FALSE;
+}
+
+static int start_stream(struct userdata *u)
+{
+	union {
+		bt_audio_msg_header_t rsp;
+		struct bt_start_stream_req start_req;
+		struct bt_start_stream_rsp start_rsp;
+		struct bt_new_stream_ind streamfd_ind;
+		bt_audio_error_t error;
+		uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
+	} msg;
+
+	assert(u);
+
+	if (u->stream_fd >= 0)
+		return 0;
+	if (u->stream_watch != 0) {
+		g_source_remove(u->stream_watch);
+		u->stream_watch = 0;
+	}
+	if (u->stream_channel != 0) {
+		g_io_channel_unref(u->stream_channel);
+		u->stream_channel = NULL;
+	}
+
+	memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
+	msg.start_req.h.type = BT_REQUEST;
+	msg.start_req.h.name = BT_START_STREAM;
+	msg.start_req.h.length = sizeof(msg.start_req);
+
+	if (service_send(u, &msg.start_req.h) < 0)
+		return -1;
+
+	msg.rsp.length = sizeof(msg.start_rsp);
+	if (service_expect(u, &msg.rsp, BT_START_STREAM) < 0)
+		return -1;
+
+	msg.rsp.length = sizeof(msg.streamfd_ind);
+	if (service_expect(u, &msg.rsp, BT_NEW_STREAM) < 0)
+		return -1;
+
+	if ((u->stream_fd = bt_audio_service_get_data_fd(u->service_fd)) < 0) {
+		DBG("Failed to get stream fd from audio service.");
+		return -1;
+	}
+
+	make_fd_nonblock(u->stream_fd);
+	make_socket_low_delay(u->stream_fd);
+
+	assert(u->stream_channel = g_io_channel_unix_new(u->stream_fd));
+
+	u->stream_watch = g_io_add_watch(u->stream_channel,
+					G_IO_IN|G_IO_OUT|G_IO_ERR|G_IO_HUP|G_IO_NVAL,
+					stream_cb, u);
+
+	return 0;
+}
+
+static int stop_stream(struct userdata *u)
+{
+	union {
+		bt_audio_msg_header_t rsp;
+		struct bt_stop_stream_req stop_req;
+		struct bt_stop_stream_rsp stop_rsp;
+		bt_audio_error_t error;
+		uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
+	} msg;
+	int r = 0;
+
+	if (u->stream_fd < 0)
+		return 0;
+
+	assert(u);
+	assert(u->stream_channel);
+
+	g_source_remove(u->stream_watch);
+	u->stream_watch = 0;
+	g_io_channel_unref(u->stream_channel);
+	u->stream_channel = NULL;
+
+	memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
+	msg.stop_req.h.type = BT_REQUEST;
+	msg.stop_req.h.name = BT_STOP_STREAM;
+	msg.stop_req.h.length = sizeof(msg.stop_req);
+
+	if (service_send(u, &msg.stop_req.h) < 0) {
+		r = -1;
+		goto done;
+	}
+
+	msg.rsp.length = sizeof(msg.stop_rsp);
+	if (service_expect(u, &msg.rsp, BT_STOP_STREAM) < 0)
+		r = -1;
+
+done:
+	close(u->stream_fd);
+	u->stream_fd = -1;
+
+	return r;
+}
+
+static gboolean sleep_cb(gpointer data)
+{
+	struct userdata *u;
+
+	assert(u = data);
+
+	u->gin_watch = g_io_add_watch(u->gin,
+		G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL, input_cb, data);
+
+	printf(">>> ");
+	fflush(stdout);
+
+	return FALSE;
+}
+
+static gboolean input_cb(GIOChannel *gin, GIOCondition condition, gpointer data)
+{
+	char *line, *tmp;
+	gsize term_pos;
+	GError *error = NULL;
+	struct userdata *u;
+	int success;
+
+	assert(u = data);
+	if (!(condition & G_IO_IN)) {
+		DBG("Got %d", condition);
+		g_main_loop_quit(main_loop);
+		return FALSE;
+	}
+
+	if (g_io_channel_read_line(gin, &line, NULL, &term_pos, &error) !=
+		G_IO_STATUS_NORMAL)
+		return FALSE;
+
+	line[term_pos] = '\0';
+	g_strstrip(line);
+	if ((tmp = strchr(line, '#')))
+		*tmp = '\0';
+	success = FALSE;
+
+#define IF_CMD(cmd) \
+	if (!success && (success = (strncmp(line, #cmd, strlen(#cmd)) == 0)))
+
+	IF_CMD(quit) {
+		g_main_loop_quit(main_loop);
+		return FALSE;
+	}
+
+	IF_CMD(sleep) {
+		unsigned int seconds;
+		if (sscanf(line, "%*s %d", &seconds) != 1)
+			DBG("sleep SECONDS");
+		else {
+			g_source_remove(u->gin_watch);
+			g_timeout_add_seconds(seconds, sleep_cb, u);
+			return FALSE;
+		}
+	}
+
+	IF_CMD(debug) {
+		char *what = NULL;
+		int enable;
+
+		if (sscanf(line, "%*s %as %d", &what, &enable) != 1)
+			DBG("debug [stream_read|stream_write] [0|1]");
+		if (strncmp(what, "stream_read", 12) == 0) {
+			u->debug_stream_read = enable;
+		} else if (strncmp(what, "stream_write", 13) == 0) {
+			u->debug_stream_write = enable;
+		} else {
+			DBG("debug [stream_read|stream_write] [0|1]");
+		}
+	}
+
+	IF_CMD(init_bt) {
+		DBG("%d", init_bt(u));
+	}
+
+	IF_CMD(init_profile) {
+		DBG("%d", init_profile(u));
+	}
+
+	IF_CMD(start_stream) {
+		DBG("%d", start_stream(u));
+	}
+
+	IF_CMD(stop_stream) {
+		DBG("%d", stop_stream(u));
+	}
+
+	IF_CMD(shutdown_bt) {
+		shutdown_bt(u);
+	}
+
+	IF_CMD(rate) {
+		if (sscanf(line, "%*s %d", &u->rate) != 1)
+			DBG("set with rate RATE");
+		DBG("rate %d", u->rate);
+	}
+
+	IF_CMD(bdaddr) {
+		char *address;
+
+		if (sscanf(line, "%*s %as", &address) != 1)
+			DBG("set with bdaddr BDADDR");
+
+		free(u->address);
+
+		u->address = address;
+		DBG("bdaddr %s", u->address);
+	}
+
+	IF_CMD(profile) {
+		char *profile = NULL;
+
+		if (sscanf(line, "%*s %as", &profile) != 1)
+			DBG("set with profile [hsp|a2dp]");
+		if (strncmp(profile, "hsp", 4) == 0) {
+			u->transport = BT_CAPABILITIES_TRANSPORT_SCO;
+		} else if (strncmp(profile, "a2dp", 5) == 0) {
+			u->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
+		} else {
+			DBG("set with profile [hsp|a2dp]");
+		}
+
+		free(profile);
+		DBG("profile %s", u->transport == BT_CAPABILITIES_TRANSPORT_SCO ?
+			"hsp" : "a2dp");
+	}
+
+	if (!success && strlen(line) != 0) {
+		DBG("%s, unknown command", line);
+	}
+
+	printf(">>> ");
+	fflush(stdout);
+	return TRUE;
+}
+
+
+static void show_usage(char* prgname)
+{
+	printf("%s: ipctest [--interactive] BDADDR\n", basename(prgname));
+}
+
+static void sig_term(int sig)
+{
+	g_main_loop_quit(main_loop);
+}
+
+int main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		show_usage(argv[0]);
+		exit(EXIT_FAILURE);
+	}
+
+	assert(main_loop = g_main_loop_new(NULL, FALSE));
+
+	if (strncmp("--interactive", argv[1], 14) == 0) {
+		if (argc < 3) {
+			show_usage(argv[0]);
+			exit(EXIT_FAILURE);
+		}
+
+		data.address = strdup(argv[2]);
+
+		signal(SIGTERM, sig_term);
+		signal(SIGINT, sig_term);
+
+		assert(data.gin = g_io_channel_unix_new(fileno(stdin)));
+
+		data.gin_watch = g_io_add_watch(data.gin,
+			G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL, input_cb, &data);
+
+		printf(">>> ");
+		fflush(stdout);
+
+		g_main_loop_run(main_loop);
+
+	} else {
+		data.address = strdup(argv[1]);
+
+		assert(init_bt(&data) == 0);
+
+		assert(init_profile(&data) == 0);
+
+		assert(start_stream(&data) == 0);
+
+		g_main_loop_run(main_loop);
+
+		assert(stop_stream(&data) == 0);
+
+		shutdown_bt(&data);
+	}
+
+	g_main_loop_unref(main_loop);
+
+	printf("\nExiting\n");
+
+	exit(EXIT_SUCCESS);
+
+	return 0;
+}
diff --git a/test/l2test.c b/test/l2test.c
new file mode 100644
index 0000000..3ac332f
--- /dev/null
+++ b/test/l2test.c
@@ -0,0 +1,1379 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2000-2001  Qualcomm Incorporated
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <ctype.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <syslog.h>
+#include <signal.h>
+#include <sys/time.h>
+#include <sys/poll.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/l2cap.h>
+
+#define NIBBLE_TO_ASCII(c)  ((c) < 0x0a ? (c) + 0x30 : (c) + 0x57)
+
+/* Test modes */
+enum {
+	SEND,
+	RECV,
+	RECONNECT,
+	MULTY,
+	DUMP,
+	CONNECT,
+	CRECV,
+	LSEND,
+	SENDDUMP,
+	LSENDDUMP,
+	LSENDRECV,
+	CSENDRECV,
+	INFOREQ,
+	PAIRING,
+};
+
+static unsigned char *buf;
+
+/* Default mtu */
+static int imtu = 672;
+static int omtu = 0;
+
+/* Default FCS option */
+static int fcs = 0x01;
+
+/* Default Transmission Window */
+static int txwin_size = 63;
+
+/* Default Max Transmission */
+static int max_transmit = 3;
+
+/* Default data size */
+static long data_size = -1;
+static long buffer_size = 2048;
+
+/* Default addr and psm and cid */
+static bdaddr_t bdaddr;
+static unsigned short psm = 0x1011;
+static unsigned short cid = 0;
+
+/* Default number of frames to send (-1 = infinite) */
+static int num_frames = -1;
+
+/* Default number of consecutive frames before the delay */
+static int count = 1;
+
+/* Default delay after sending count number of frames */
+static unsigned long delay = 0;
+
+static char *filename = NULL;
+
+static int rfcmode = 0;
+static int master = 0;
+static int auth = 0;
+static int encrypt = 0;
+static int secure = 0;
+static int socktype = SOCK_SEQPACKET;
+static int linger = 0;
+static int reliable = 0;
+static int timestamp = 0;
+static int defer_setup = 0;
+
+static float tv2fl(struct timeval tv)
+{
+	return (float)tv.tv_sec + (float)(tv.tv_usec/1000000.0);
+}
+
+static char *ltoh(unsigned long c, char* s)
+{
+	int c1;
+
+	c1     = (c >> 28) & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	c1     = (c >> 24) & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	c1     = (c >> 20) & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	c1     = (c >> 16) & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	c1     = (c >> 12) & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	c1     = (c >>  8) & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	c1     = (c >>  4) & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	c1     = c & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	*s     = 0;
+	return s;
+}
+
+static char *ctoh(char c, char* s)
+{
+	char c1;
+
+	c1     = (c >> 4) & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	c1     = c & 0x0f;
+	*(s++) = NIBBLE_TO_ASCII (c1);
+	*s     = 0;
+	return s;
+}
+
+static void hexdump(unsigned char *s, unsigned long l)
+{
+	char bfr[80];
+	char *pb;
+	unsigned long i, n = 0;
+
+	if (l == 0)
+		return;
+
+	while (n < l) {
+		pb = bfr;
+		pb = ltoh (n, pb);
+		*(pb++) = ':';
+		*(pb++) = ' ';
+		for (i = 0; i < 16; i++) {
+			if (n + i >= l) {
+				*(pb++) = ' ';
+				*(pb++) = ' ';
+			} else
+				pb = ctoh (*(s + i), pb);
+			*(pb++) = ' ';
+		}
+		*(pb++) = ' ';
+		for (i = 0; i < 16; i++) {
+			if (n + i >= l)
+				break;
+			else
+				*(pb++) = (isprint (*(s + i)) ? *(s + i) : '.');
+		}
+		*pb = 0;
+		n += 16;
+		s += 16;
+		puts(bfr);
+	}
+}
+
+static int do_connect(char *svr)
+{
+	struct sockaddr_l2 addr;
+	struct l2cap_options opts;
+	struct l2cap_conninfo conn;
+	socklen_t optlen;
+	int sk, opt;
+
+	/* Create socket */
+	sk = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP);
+	if (sk < 0) {
+		syslog(LOG_ERR, "Can't create socket: %s (%d)",
+							strerror(errno), errno);
+		return -1;
+	}
+
+	/* Bind to local address */
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, &bdaddr);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Get default options */
+	memset(&opts, 0, sizeof(opts));
+	optlen = sizeof(opts);
+
+	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get default L2CAP options: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Set new options */
+	opts.omtu = omtu;
+	opts.imtu = imtu;
+	opts.mode = rfcmode;
+
+	opts.fcs = fcs;
+	opts.txwin_size = txwin_size;
+	opts.max_tx = max_transmit;
+
+	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) {
+		syslog(LOG_ERR, "Can't set L2CAP options: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+#if 0
+	/* Enable SO_TIMESTAMP */
+	if (timestamp) {
+		int t = 1;
+
+		if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
+			syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
+							strerror(errno), errno);
+			goto error;
+		}
+	}
+#endif
+
+	/* Enable SO_LINGER */
+	if (linger) {
+		struct linger l = { .l_onoff = 1, .l_linger = linger };
+
+		if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
+			syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
+							strerror(errno), errno);
+			goto error;
+		}
+	}
+
+	/* Set link mode */
+	opt = 0;
+	if (reliable)
+		opt |= L2CAP_LM_RELIABLE;
+	if (master)
+		opt |= L2CAP_LM_MASTER;
+	if (auth)
+		opt |= L2CAP_LM_AUTH;
+	if (encrypt)
+		opt |= L2CAP_LM_ENCRYPT;
+	if (secure)
+		opt |= L2CAP_LM_SECURE;
+
+	if (setsockopt(sk, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {
+		syslog(LOG_ERR, "Can't set L2CAP link mode: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Connect to remote device */
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	str2ba(svr, &addr.l2_bdaddr);
+	if (cid)
+		addr.l2_cid = htobs(cid);
+	else if (psm)
+		addr.l2_psm = htobs(psm);
+	else
+		goto error;
+
+	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0 ) {
+		syslog(LOG_ERR, "Can't connect: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Get current options */
+	memset(&opts, 0, sizeof(opts));
+	optlen = sizeof(opts);
+
+	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get L2CAP options: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Get connection information */
+	memset(&conn, 0, sizeof(conn));
+	optlen = sizeof(conn);
+
+	if (getsockopt(sk, SOL_L2CAP, L2CAP_CONNINFO, &conn, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get L2CAP connection information: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	syslog(LOG_INFO, "Connected [imtu %d, omtu %d, flush_to %d, "
+				"mode %d, handle %d, class 0x%02x%02x%02x]",
+		opts.imtu, opts.omtu, opts.flush_to, opts.mode, conn.hci_handle,
+		conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
+
+	omtu = (opts.omtu > buffer_size) ? buffer_size : opts.omtu;
+	imtu = (opts.imtu > buffer_size) ? buffer_size : opts.imtu;
+
+	return sk;
+
+error:
+	close(sk);
+	return -1;
+}
+
+static void do_listen(void (*handler)(int sk))
+{
+	struct sockaddr_l2 addr;
+	struct l2cap_options opts;
+	struct l2cap_conninfo conn;
+	socklen_t optlen;
+	int sk, nsk, opt;
+	char ba[18];
+
+	/* Create socket */
+	sk = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP);
+	if (sk < 0) {
+		syslog(LOG_ERR, "Can't create socket: %s (%d)",
+							strerror(errno), errno);
+		exit(1);
+	}
+
+	/* Bind to local address */
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, &bdaddr);
+	if (cid)
+		addr.l2_cid = htobs(cid);
+	else if (psm)
+		addr.l2_psm = htobs(psm);
+	else
+		goto error;
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Set link mode */
+	opt = 0;
+	if (reliable)
+		opt |= L2CAP_LM_RELIABLE;
+	if (master)
+		opt |= L2CAP_LM_MASTER;
+	if (auth)
+		opt |= L2CAP_LM_AUTH;
+	if (encrypt)
+		opt |= L2CAP_LM_ENCRYPT;
+	if (secure)
+		opt |= L2CAP_LM_SECURE;
+
+	if (opt && setsockopt(sk, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {
+		syslog(LOG_ERR, "Can't set L2CAP link mode: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Get default options */
+	memset(&opts, 0, sizeof(opts));
+	optlen = sizeof(opts);
+
+	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get default L2CAP options: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Set new options */
+	opts.omtu = omtu;
+	opts.imtu = imtu;
+	if (rfcmode > 0)
+		opts.mode = rfcmode;
+
+	opts.fcs = fcs;
+	opts.txwin_size = txwin_size;
+	opts.max_tx = max_transmit;
+
+	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) {
+		syslog(LOG_ERR, "Can't set L2CAP options: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	if (socktype == SOCK_DGRAM) {
+		handler(sk);
+		return;
+	}
+
+	/* Enable deferred setup */
+	opt = defer_setup;
+
+	if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
+						&opt, sizeof(opt)) < 0) {
+		syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Listen for connections */
+	if (listen(sk, 10)) {
+		syslog(LOG_ERR, "Can not listen on the socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Check for socket address */
+	memset(&addr, 0, sizeof(addr));
+	optlen = sizeof(addr);
+
+	if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get socket name: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	psm = btohs(addr.l2_psm);
+	cid = btohs(addr.l2_cid);
+
+	syslog(LOG_INFO, "Waiting for connection on psm %d ...", psm);
+
+	while (1) {
+		memset(&addr, 0, sizeof(addr));
+		optlen = sizeof(addr);
+
+		nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
+		if (nsk < 0) {
+			syslog(LOG_ERR, "Accept failed: %s (%d)",
+							strerror(errno), errno);
+			goto error;
+		}
+		if (fork()) {
+			/* Parent */
+			close(nsk);
+			continue;
+		}
+		/* Child */
+		close(sk);
+
+		/* Get current options */
+		memset(&opts, 0, sizeof(opts));
+		optlen = sizeof(opts);
+
+		if (getsockopt(nsk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {
+			syslog(LOG_ERR, "Can't get L2CAP options: %s (%d)",
+							strerror(errno), errno);
+			if (!defer_setup) {
+				close(nsk);
+				goto error;
+			}
+		}
+
+		/* Get connection information */
+		memset(&conn, 0, sizeof(conn));
+		optlen = sizeof(conn);
+
+		if (getsockopt(nsk, SOL_L2CAP, L2CAP_CONNINFO, &conn, &optlen) < 0) {
+			syslog(LOG_ERR, "Can't get L2CAP connection information: %s (%d)",
+							strerror(errno), errno);
+			if (!defer_setup) {
+				close(nsk);
+				goto error;
+			}
+		}
+
+		ba2str(&addr.l2_bdaddr, ba);
+		syslog(LOG_INFO, "Connect from %s [imtu %d, omtu %d, flush_to %d, "
+					"mode %d, handle %d, class 0x%02x%02x%02x]",
+			ba, opts.imtu, opts.omtu, opts.flush_to, opts.mode, conn.hci_handle,
+			conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
+
+		omtu = (opts.omtu > buffer_size) ? buffer_size : opts.omtu;
+		imtu = (opts.imtu > buffer_size) ? buffer_size : opts.imtu;
+
+#if 0
+		/* Enable SO_TIMESTAMP */
+		if (timestamp) {
+			int t = 1;
+
+			if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
+				syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
+							strerror(errno), errno);
+				goto error;
+			}
+		}
+#endif
+
+		/* Enable SO_LINGER */
+		if (linger) {
+			struct linger l = { .l_onoff = 1, .l_linger = linger };
+
+			if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
+				syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
+							strerror(errno), errno);
+				close(nsk);
+				goto error;
+			}
+		}
+
+		/* Handle deferred setup */
+		if (defer_setup) {
+			syslog(LOG_INFO, "Waiting for %d seconds",
+							abs(defer_setup) - 1);
+			sleep(abs(defer_setup) - 1);
+
+			if (defer_setup < 0) {
+				close(nsk);
+				goto error;
+			}
+		}
+
+		handler(nsk);
+
+		syslog(LOG_INFO, "Disconnect: %m");
+		exit(0);
+	}
+
+	return;
+
+error:
+	close(sk);
+	exit(1);
+}
+
+static void dump_mode(int sk)
+{
+	socklen_t optlen;
+	int opt, len;
+
+	if (data_size < 0)
+		data_size = imtu;
+
+	if (defer_setup) {
+		len = read(sk, buf, sizeof(buf));
+		if (len < 0)
+			syslog(LOG_ERR, "Initial read error: %s (%d)",
+						strerror(errno), errno);
+		else
+			syslog(LOG_INFO, "Initial bytes %d", len);
+	}
+
+	syslog(LOG_INFO, "Receiving ...");
+	while (1) {
+		fd_set rset;
+
+		FD_ZERO(&rset);
+		FD_SET(sk, &rset);
+
+		if (select(sk + 1, &rset, NULL, NULL, NULL) < 0)
+			return;
+
+		if (!FD_ISSET(sk, &rset))
+			continue;
+
+		len = read(sk, buf, data_size);
+		if (len <= 0) {
+			if (len < 0) {
+				if (reliable && (errno == ECOMM)) {
+					syslog(LOG_INFO, "L2CAP Error ECOMM - clearing error and continuing.");
+					optlen = sizeof(opt);
+					if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &opt, &optlen) < 0) {
+						syslog(LOG_ERR, "Couldn't getsockopt(SO_ERROR): %s (%d)",
+							strerror(errno), errno);
+						return;
+					}
+					continue;
+				} else {
+					syslog(LOG_ERR, "Read error: %s(%d)",
+							strerror(errno), errno);
+				}
+			}
+			return;
+		}
+
+		syslog(LOG_INFO, "Recevied %d bytes", len);
+		hexdump(buf, len);
+	}
+}
+
+static void recv_mode(int sk)
+{
+	struct timeval tv_beg, tv_end, tv_diff;
+	struct pollfd p;
+	char ts[30];
+	long total;
+	uint32_t seq;
+	socklen_t optlen;
+	int opt, len;
+
+	if (data_size < 0)
+		data_size = imtu;
+
+	if (defer_setup) {
+		len = read(sk, buf, sizeof(buf));
+		if (len < 0)
+			syslog(LOG_ERR, "Initial read error: %s (%d)",
+						strerror(errno), errno);
+		else
+			syslog(LOG_INFO, "Initial bytes %d", len);
+	}
+
+	syslog(LOG_INFO, "Receiving ...");
+
+	memset(ts, 0, sizeof(ts));
+
+	p.fd = sk;
+	p.events = POLLIN | POLLERR | POLLHUP;
+
+	seq = 0;
+	while (1) {
+		gettimeofday(&tv_beg, NULL);
+		total = 0;
+		while (total < data_size) {
+			uint32_t sq;
+			uint16_t l;
+			int i;
+
+			p.revents = 0;
+			if (poll(&p, 1, -1) <= 0)
+				return;
+
+			if (p.revents & (POLLERR | POLLHUP))
+				return;
+
+			len = recv(sk, buf, data_size, 0);
+			if (len < 0) {
+				if (reliable && (errno == ECOMM)) {
+					syslog(LOG_INFO, "L2CAP Error ECOMM - clearing error and continuing.\n");
+					optlen = sizeof(opt);
+					if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &opt, &optlen) < 0) {
+						syslog(LOG_ERR, "Couldn't getsockopt(SO_ERROR): %s (%d)",
+							strerror(errno), errno);
+						return;
+					}
+					continue;
+				} else {
+					syslog(LOG_ERR, "Read failed: %s (%d)",
+						strerror(errno), errno);
+				}
+			}
+
+			if (len < 6)
+				break;
+
+			if (timestamp) {
+				struct timeval tv;
+
+				if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
+					timestamp = 0;
+					memset(ts, 0, sizeof(ts));
+				} else {
+					sprintf(ts, "[%ld.%ld] ",
+							tv.tv_sec, tv.tv_usec);
+				}
+			}
+
+			/* Check sequence */
+			sq = btohl(*(uint32_t *) buf);
+			if (seq != sq) {
+				syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
+				seq = sq;
+			}
+			seq++;
+
+			/* Check length */
+			l = btohs(*(uint16_t *) (buf + 4));
+			if (len != l) {
+				syslog(LOG_INFO, "size missmatch: %d -> %d", len, l);
+				continue;
+			}
+
+			/* Verify data */
+			for (i = 6; i < len; i++) {
+				if (buf[i] != 0x7f)
+					syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
+			}
+
+			total += len;
+		}
+		gettimeofday(&tv_end, NULL);
+
+		timersub(&tv_end, &tv_beg, &tv_diff);
+
+		syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
+			tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
+	}
+}
+
+static void do_send(int sk)
+{
+	uint32_t seq;
+	int i, fd, len, buflen, size, sent;
+
+	syslog(LOG_INFO, "Sending ...");
+
+	if (data_size < 0)
+		data_size = omtu;
+
+	if (filename) {
+		fd = open(filename, O_RDONLY);
+		if (fd < 0) {
+			syslog(LOG_ERR, "Open failed: %s (%d)",
+							strerror(errno), errno);
+			exit(1);
+		}
+
+		sent = 0;
+		size = read(fd, buf, data_size);
+		while (size > 0) {
+			buflen = (size > omtu) ? omtu : size;
+
+			len = send(sk, buf + sent, buflen, 0);
+
+			sent += len;
+			size -= len;
+		}
+		return;
+	} else {
+		for (i = 6; i < data_size; i++)
+			buf[i] = 0x7f;
+	}
+
+	seq = 0;
+	while ((num_frames == -1) || (num_frames-- > 0)) {
+		*(uint32_t *) buf = htobl(seq);
+		*(uint16_t *) (buf + 4) = htobs(data_size);
+		seq++;
+
+		sent = 0;
+		size = data_size;
+		while (size > 0) {
+			buflen = (size > omtu) ? omtu : size;
+
+			len = send(sk, buf, buflen, 0);
+			if (len < 0 || len != buflen) {
+				syslog(LOG_ERR, "Send failed: %s (%d)",
+							strerror(errno), errno);
+				exit(1);
+			}
+
+			sent += len;
+			size -= len;
+		}
+
+		if (num_frames && delay && count && !(seq % count))
+			usleep(delay);
+	}
+}
+
+static void send_mode(int sk)
+{
+	do_send(sk);
+
+	syslog(LOG_INFO, "Closing channel ...");
+	if (shutdown(sk, SHUT_RDWR) < 0)
+		syslog(LOG_INFO, "Close failed: %m");
+	else
+		syslog(LOG_INFO, "Done");
+}
+
+static void senddump_mode(int sk)
+{
+	do_send(sk);
+
+	dump_mode(sk);
+}
+
+static void send_and_recv_mode(int sk)
+{
+	int flags;
+
+	if ((flags = fcntl(sk, F_GETFL, 0)) < 0)
+		flags = 0;
+	fcntl(sk, F_SETFL, flags | O_NONBLOCK);
+
+	/* fork for duplex channel */
+	if (fork())
+		send_mode(sk);
+	else
+		recv_mode(sk);
+	return;
+}
+
+static void reconnect_mode(char *svr)
+{
+	while (1) {
+		int sk = do_connect(svr);
+		close(sk);
+	}
+}
+
+static void connect_mode(char *svr)
+{
+	struct pollfd p;
+	int sk;
+
+	if ((sk = do_connect(svr)) < 0)
+		exit(1);
+
+	p.fd = sk;
+	p.events = POLLERR | POLLHUP;
+
+	while (1) {
+		p.revents = 0;
+		if (poll(&p, 1, 500))
+			break;
+	}
+
+	syslog(LOG_INFO, "Disconnected");
+
+	close(sk);
+}
+
+static void multi_connect_mode(int argc, char *argv[])
+{
+	int i, n, sk;
+
+	while (1) {
+		for (n = 0; n < argc; n++) {
+			for (i = 0; i < count; i++) {
+				if (fork())
+					continue;
+
+				/* Child */
+				sk = do_connect(argv[n]);
+				usleep(500);
+				close(sk);
+				exit(0);
+			}
+		}
+		sleep(4);
+	}
+}
+
+static void info_request(char *svr)
+{
+	unsigned char buf[48];
+	l2cap_cmd_hdr *cmd = (l2cap_cmd_hdr *) buf;
+	l2cap_info_req *req = (l2cap_info_req *) (buf + L2CAP_CMD_HDR_SIZE);
+	l2cap_info_rsp *rsp = (l2cap_info_rsp *) (buf + L2CAP_CMD_HDR_SIZE);
+	uint16_t mtu;
+	uint32_t channels, mask = 0x0000;
+	struct sockaddr_l2 addr;
+	int sk, err;
+
+	sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
+	if (sk < 0) {
+		perror("Can't create socket");
+		return;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, &bdaddr);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		perror("Can't bind socket");
+		goto failed;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	str2ba(svr, &addr.l2_bdaddr);
+
+	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0 ) {
+		perror("Can't connect socket");
+		goto failed;
+	}
+
+	memset(buf, 0, sizeof(buf));
+	cmd->code  = L2CAP_INFO_REQ;
+	cmd->ident = 141;
+	cmd->len   = htobs(2);
+	req->type  = htobs(0x0001);
+
+	if (send(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_REQ_SIZE, 0) < 0) {
+		perror("Can't send info request");
+		goto failed;
+	}
+
+	err = recv(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + 2, 0);
+	if (err < 0) {
+		perror("Can't receive info response");
+		goto failed;
+	}
+
+	switch (btohs(rsp->result)) {
+	case 0x0000:
+		memcpy(&mtu, rsp->data, sizeof(mtu));
+		printf("Connectionless MTU size is %d\n", btohs(mtu));
+		break;
+	case 0x0001:
+		printf("Connectionless MTU is not supported\n");
+		break;
+	}
+
+	memset(buf, 0, sizeof(buf));
+	cmd->code  = L2CAP_INFO_REQ;
+	cmd->ident = 142;
+	cmd->len   = htobs(2);
+	req->type  = htobs(0x0002);
+
+	if (send(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_REQ_SIZE, 0) < 0) {
+		perror("Can't send info request");
+		goto failed;
+	}
+
+	err = recv(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + 4, 0);
+	if (err < 0) {
+		perror("Can't receive info response");
+		goto failed;
+	}
+
+	switch (btohs(rsp->result)) {
+	case 0x0000:
+		memcpy(&mask, rsp->data, sizeof(mask));
+		printf("Extended feature mask is 0x%04x\n", btohl(mask));
+		if (mask & 0x01)
+			printf("  Flow control mode\n");
+		if (mask & 0x02)
+			printf("  Retransmission mode\n");
+		if (mask & 0x04)
+			printf("  Bi-directional QoS\n");
+		if (mask & 0x08)
+			printf("  Enhanced Retransmission mode\n");
+		if (mask & 0x10)
+			printf("  Streaming mode\n");
+		if (mask & 0x20)
+			printf("  FCS Option\n");
+		if (mask & 0x40)
+			printf("  Extended Flow Specification\n");
+		if (mask & 0x80)
+			printf("  Fixed Channels\n");
+		if (mask & 0x0100)
+			printf("  Extended Window Size\n");
+		if (mask & 0x0200)
+			printf("  Unicast Connectionless Data Reception\n");
+		break;
+	case 0x0001:
+		printf("Extended feature mask is not supported\n");
+		break;
+	}
+
+	if (!(mask & 0x80))
+		goto failed;
+
+	memset(buf, 0, sizeof(buf));
+	cmd->code  = L2CAP_INFO_REQ;
+	cmd->ident = 143;
+	cmd->len   = htobs(2);
+	req->type  = htobs(0x0003);
+
+	if (send(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_REQ_SIZE, 0) < 0) {
+		perror("Can't send info request");
+		goto failed;
+	}
+
+	err = recv(sk, buf, L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + 8, 0);
+	if (err < 0) {
+		perror("Can't receive info response");
+		goto failed;
+	}
+
+	switch (btohs(rsp->result)) {
+	case 0x0000:
+		memcpy(&channels, rsp->data, sizeof(channels));
+		printf("Fixed channels list is 0x%04x\n", btohl(channels));
+		break;
+	case 0x0001:
+		printf("Fixed channels list is not supported\n");
+		break;
+	}
+
+failed:
+	close(sk);
+}
+
+static void do_pairing(char *svr)
+{
+	struct sockaddr_l2 addr;
+	int sk, opt;
+
+	sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
+	if (sk < 0) {
+		perror("Can't create socket");
+		return;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, &bdaddr);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		perror("Can't bind socket");
+		goto failed;
+	}
+
+	if (secure)
+		opt = L2CAP_LM_SECURE;
+	else
+		opt = L2CAP_LM_ENCRYPT;
+
+	if (setsockopt(sk, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {
+		perror("Can't set link mode");
+		goto failed;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	str2ba(svr, &addr.l2_bdaddr);
+
+	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0 ) {
+		perror("Can't connect socket");
+		goto failed;
+	}
+
+	printf("Pairing successful\n");
+
+failed:
+	close(sk);
+}
+
+static void usage(void)
+{
+	printf("l2test - L2CAP testing\n"
+		"Usage:\n");
+	printf("\tl2test <mode> [options] [bdaddr]\n");
+	printf("Modes:\n"
+		"\t-r listen and receive\n"
+		"\t-w listen and send\n"
+		"\t-d listen and dump incoming data\n"
+		"\t-x listen, then send, then dump incoming data\n"
+		"\t-t listen, then send and receive at the same time\n"
+		"\t-q connect, then send and receive at the same time\n"
+		"\t-s connect and send\n"
+		"\t-u connect and receive\n"
+		"\t-n connect and be silent\n"
+		"\t-y connect, then send, then dump incoming data\n"
+		"\t-c connect, disconnect, connect, ...\n"
+		"\t-m multiple connects\n"
+		"\t-p trigger dedicated bonding\n"
+		"\t-z information request\n");
+
+	printf("Options:\n"
+		"\t[-b bytes] [-i device] [-P psm] [-J cid]\n"
+		"\t[-I imtu] [-O omtu]\n"
+		"\t[-L seconds] enable SO_LINGER\n"
+		"\t[-W seconds] enable deferred setup\n"
+		"\t[-B filename] use data packets from file\n"
+		"\t[-N num] send num frames (default = infinite)\n"
+		"\t[-C num] send num frames before delay (default = 1)\n"
+		"\t[-D milliseconds] delay after sending num frames (default = 0)\n"
+		"\t[-X mode] select retransmission/flow-control mode\n"
+		"\t[-F fcs] use CRC16 check (default = 1)\n"
+		"\t[-Q num] Max Transmit value (default = 3)\n"
+		"\t[-Z size] Transmission Window size (default = 63)\n"
+		"\t[-R] reliable mode\n"
+		"\t[-G] use connectionless channel (datagram)\n"
+		"\t[-U] use sock stream\n"
+		"\t[-A] request authentication\n"
+		"\t[-E] request encryption\n"
+		"\t[-S] secure connection\n"
+		"\t[-M] become master\n"
+		"\t[-T] enable timestamps\n");
+}
+
+int main(int argc, char *argv[])
+{
+	struct sigaction sa;
+	int opt, sk, mode = RECV, need_addr = 0;
+
+	bacpy(&bdaddr, BDADDR_ANY);
+
+	while ((opt=getopt(argc,argv,"rdscuwmntqxyzpb:i:P:I:O:J:B:N:L:W:C:D:X:F:Q:Z:RUGAESMT")) != EOF) {
+		switch(opt) {
+		case 'r':
+			mode = RECV;
+			break;
+
+		case 's':
+			mode = SEND;
+			need_addr = 1;
+			break;
+
+		case 'w':
+			mode = LSEND;
+			break;
+
+		case 'u':
+			mode = CRECV;
+			need_addr = 1;
+			break;
+
+		case 'd':
+			mode = DUMP;
+			break;
+
+		case 'c':
+			mode = RECONNECT;
+			need_addr = 1;
+			break;
+
+		case 'n':
+			mode = CONNECT;
+			need_addr = 1;
+			break;
+
+		case 'm':
+			mode = MULTY;
+			need_addr = 1;
+			break;
+
+		case 't':
+			mode = LSENDRECV;
+			break;
+
+		case 'q':
+			mode = CSENDRECV;
+			need_addr = 1;
+			break;
+
+		case 'x':
+			mode = LSENDDUMP;
+			break;
+
+		case 'y':
+			mode = SENDDUMP;
+			break;
+
+		case 'z':
+			mode = INFOREQ;
+			need_addr = 1;
+			break;
+
+		case 'p':
+			mode = PAIRING;
+			need_addr = 1;
+			break;
+
+		case 'b':
+			data_size = atoi(optarg);
+			break;
+
+		case 'i':
+			if (!strncasecmp(optarg, "hci", 3))
+				hci_devba(atoi(optarg + 3), &bdaddr);
+			else
+				str2ba(optarg, &bdaddr);
+			break;
+
+		case 'P':
+			psm = atoi(optarg);
+			break;
+
+		case 'I':
+			imtu = atoi(optarg);
+			break;
+
+		case 'O':
+			omtu = atoi(optarg);
+			break;
+
+		case 'L':
+			linger = atoi(optarg);
+			break;
+
+		case 'W':
+			defer_setup = atoi(optarg);
+			break;
+
+		case 'B':
+			filename = strdup(optarg);
+			break;
+
+		case 'N':
+			num_frames = atoi(optarg);
+			break;
+
+		case 'C':
+			count = atoi(optarg);
+			break;
+
+		case 'D':
+			delay = atoi(optarg) * 1000;
+			break;
+
+		case 'X':
+			if (strcasecmp(optarg, "ertm") == 0)
+				rfcmode = L2CAP_MODE_ERTM;
+			else
+				rfcmode = atoi(optarg);
+			break;
+
+		case 'F':
+			fcs = atoi(optarg);
+			break;
+
+		case 'R':
+			reliable = 1;
+			break;
+
+		case 'M':
+			master = 1;
+			break;
+
+		case 'A':
+			auth = 1;
+			break;
+
+		case 'E':
+			encrypt = 1;
+			break;
+
+		case 'S':
+			secure = 1;
+			break;
+
+		case 'G':
+			socktype = SOCK_DGRAM;
+			break;
+
+		case 'U':
+			socktype = SOCK_STREAM;
+			break;
+
+		case 'T':
+			timestamp = 1;
+			break;
+
+		case 'Q':
+			max_transmit = atoi(optarg);
+			break;
+
+		case 'Z':
+			txwin_size = atoi(optarg);
+			break;
+
+		case 'J':
+			cid = atoi(optarg);
+			break;
+
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	if (need_addr && !(argc - optind)) {
+		usage();
+		exit(1);
+	}
+
+	if (data_size < 0)
+		buffer_size = (omtu > imtu) ? omtu : imtu;
+	else
+		buffer_size = data_size;
+
+	if (!(buf = malloc(buffer_size))) {
+		perror("Can't allocate data buffer");
+		exit(1);
+	}
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_handler = SIG_IGN;
+	sa.sa_flags   = SA_NOCLDSTOP;
+	sigaction(SIGCHLD, &sa, NULL);
+
+	openlog("l2test", LOG_PERROR | LOG_PID, LOG_LOCAL0);
+
+	switch (mode) {
+		case RECV:
+			do_listen(recv_mode);
+			break;
+
+		case CRECV:
+			sk = do_connect(argv[optind]);
+			if (sk < 0)
+				exit(1);
+			recv_mode(sk);
+			break;
+
+		case DUMP:
+			do_listen(dump_mode);
+			break;
+
+		case SEND:
+			sk = do_connect(argv[optind]);
+			if (sk < 0)
+				exit(1);
+			send_mode(sk);
+			break;
+
+		case LSEND:
+			do_listen(send_mode);
+			break;
+
+		case RECONNECT:
+			reconnect_mode(argv[optind]);
+			break;
+
+		case MULTY:
+			multi_connect_mode(argc - optind, argv + optind);
+			break;
+
+		case CONNECT:
+			connect_mode(argv[optind]);
+			break;
+
+		case SENDDUMP:
+			sk = do_connect(argv[optind]);
+			if (sk < 0)
+				exit(1);
+			senddump_mode(sk);
+			break;
+
+		case LSENDDUMP:
+			do_listen(senddump_mode);
+			break;
+
+		case LSENDRECV:
+			do_listen(send_and_recv_mode);
+			break;
+
+		case CSENDRECV:
+			sk = do_connect(argv[optind]);
+			if (sk < 0)
+				exit(1);
+
+			send_and_recv_mode(sk);
+			break;
+
+		case INFOREQ:
+			info_request(argv[optind]);
+			exit(0);
+
+		case PAIRING:
+			do_pairing(argv[optind]);
+			exit(0);
+	}
+
+	syslog(LOG_INFO, "Exit");
+
+	closelog();
+
+	return 0;
+}
diff --git a/test/list-devices b/test/list-devices
new file mode 100755
index 0000000..9120714
--- /dev/null
+++ b/test/list-devices
@@ -0,0 +1,87 @@
+#!/usr/bin/python
+
+import dbus
+
+bus = dbus.SystemBus()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+						"org.bluez.Manager")
+
+def extract_objects(object_list):
+	list = ""
+	for object in object_list:
+		val = str(object)
+		list = list + val[val.rfind("/") + 1:] + " "
+	return list
+
+def extract_uuids(uuid_list):
+	list = ""
+	for uuid in uuid_list:
+		if (uuid.endswith("-0000-1000-8000-00805f9b34fb")):
+			if (uuid.startswith("0000")):
+				val = "0x" + uuid[4:8]
+			else:
+				val = "0x" + uuid[0:8]
+		else:
+			val = str(uuid)
+		list = list + val + " "
+	return list
+
+adapter_list = manager.ListAdapters()
+
+for i in adapter_list:
+	adapter = dbus.Interface(bus.get_object("org.bluez", i),
+							"org.bluez.Adapter")
+	print "[ " + i + " ]"
+
+	properties  = adapter.GetProperties()
+	for key in properties.keys():
+		value = properties[key]
+		if (key == "Devices"):
+			list = extract_objects(value)
+			print "    %s = %s" % (key, list)
+		elif (key == "UUIDs"):
+			list = extract_uuids(value)
+			print "    %s = %s" % (key, list)
+		else:
+			print "    %s = %s" % (key, value)
+
+	try:
+		device_list = properties["Devices"]
+	except:
+		device_list = []
+
+	for n in device_list:
+		device = dbus.Interface(bus.get_object("org.bluez", n),
+							"org.bluez.Device")
+		print "    [ " + n + " ]"
+
+		properties = device.GetProperties()
+		for key in properties.keys():
+			value = properties[key]
+			if (key == "Nodes"):
+				list = extract_objects(value)
+				print "        %s = %s" % (key, list)
+			elif (key == "UUIDs"):
+				list = extract_uuids(value)
+				print "        %s = %s" % (key, list)
+			elif (key == "Class"):
+				print "        %s = 0x%06x" % (key, value)
+			else:
+				print "        %s = %s" % (key, value)
+
+		try:
+			node_list = properties["Nodes"]
+		except:
+			node_list = []
+
+		for x in node_list:
+			node = dbus.Interface(bus.get_object("org.bluez", x),
+							"org.bluez.Node")
+			print "        [ " + x + " ]"
+
+			properties = node.GetProperties()
+			for key in properties.keys():
+				print "            %s = %s" % (key, properties[key])
+
+	print
diff --git a/test/lmptest.c b/test/lmptest.c
new file mode 100644
index 0000000..549ae12
--- /dev/null
+++ b/test/lmptest.c
@@ -0,0 +1,175 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2005-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+
+#if 0
+#define OCF_ERICSSON_SEND_LMP		0x0021
+typedef struct {
+	uint16_t handle;
+	uint8_t  length;
+	uint8_t  data[17];
+} __attribute__ ((packed)) ericsson_send_lmp_cp;
+#define ERICSSON_SEND_LMP_CP_SIZE 20
+
+static int ericsson_send_lmp(int dd, uint16_t handle, uint8_t length, uint8_t *data)
+{
+	struct hci_request rq;
+	ericsson_send_lmp_cp cp;
+
+	memset(&cp, 0, sizeof(cp));
+	cp.handle = htobs(handle);
+	cp.length = length;
+	memcpy(cp.data, data, length);
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = OCF_ERICSSON_SEND_LMP;
+	rq.cparam = &cp;
+	rq.clen   = ERICSSON_SEND_LMP_CP_SIZE;
+	rq.rparam = NULL;
+	rq.rlen   = 0;
+
+	if (hci_send_req(dd, &rq, 1000) < 0)
+		return -1;
+
+	return 0;
+}
+#endif
+
+#define OCF_ERICSSON_WRITE_EVENTS	0x0043
+typedef struct {
+	uint8_t mask;
+	uint8_t opcode;
+	uint8_t opcode_ext;
+} __attribute__ ((packed)) ericsson_write_events_cp;
+#define ERICSSON_WRITE_EVENTS_CP_SIZE 3
+
+static int ericsson_write_events(int dd, uint8_t mask)
+{
+	struct hci_request rq;
+	ericsson_write_events_cp cp;
+
+	memset(&cp, 0, sizeof(cp));
+	cp.mask = mask;
+	cp.opcode = 0x00;
+	cp.opcode_ext = 0x00;
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_VENDOR_CMD;
+	rq.ocf    = OCF_ERICSSON_WRITE_EVENTS;
+	rq.cparam = &cp;
+	rq.clen   = ERICSSON_WRITE_EVENTS_CP_SIZE;
+	rq.rparam = NULL;
+	rq.rlen   = 0;
+
+	if (hci_send_req(dd, &rq, 1000) < 0)
+		return -1;
+
+	return 0;
+}
+
+static void usage(void)
+{
+	printf("lmptest - Utility for testing special LMP functions\n\n");
+	printf("Usage:\n"
+		"\tlmptest [-i <dev>]\n");
+}
+
+static struct option main_options[] = {
+	{ "device",	1, 0, 'i' },
+	{ "help",	0, 0, 'h' },
+	{ 0, 0, 0, 0 }
+};
+
+int main(int argc, char *argv[])
+{
+	struct hci_version ver;
+	int dd, opt, dev = 0;
+
+	while ((opt=getopt_long(argc, argv, "+i:h", main_options, NULL)) != -1) {
+		switch (opt) {
+		case 'i':
+			dev = hci_devid(optarg);
+			if (dev < 0) {
+				perror("Invalid device");
+				exit(1);
+			}
+			break;
+
+		case 'h':
+		default:
+			usage();
+			exit(0);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+	optind = 0;
+
+	dd = hci_open_dev(dev);
+	if (dd < 0) {
+		fprintf(stderr, "Can't open device hci%d: %s (%d)\n",
+						dev, strerror(errno), errno);
+		exit(1);
+	}
+
+	if (hci_read_local_version(dd, &ver, 1000) < 0) {
+		fprintf(stderr, "Can't read version for hci%d: %s (%d)\n",
+						dev, strerror(errno), errno);
+		hci_close_dev(dd);
+		exit(1);
+	}
+
+	if (ver.manufacturer != 37 && ver.manufacturer != 48) {
+		fprintf(stderr, "Can't find supported device hci%d: %s (%d)\n",
+						dev, strerror(ENOSYS), ENOSYS);
+		hci_close_dev(dd);
+		exit(1);
+	}
+
+	if (ericsson_write_events(dd, 0x03) < 0) {
+		fprintf(stderr, "Can't activate events for hci%d: %s (%d)\n",
+						dev, strerror(errno), errno);
+		hci_close_dev(dd);
+		exit(1);
+	}
+
+	hci_close_dev(dd);
+
+	return 0;
+}
diff --git a/test/monitor-bluetooth b/test/monitor-bluetooth
new file mode 100755
index 0000000..a5e5300
--- /dev/null
+++ b/test/monitor-bluetooth
@@ -0,0 +1,56 @@
+#!/usr/bin/python
+
+import gobject
+
+import dbus
+import dbus.mainloop.glib
+
+def property_changed(name, value, path, interface):
+	iface = interface[interface.rfind(".") + 1:]
+	val = str(value)
+	print "{%s.PropertyChanged} [%s] %s = %s" % (iface, path, name, val)
+
+def object_signal(value, path, interface, member):
+	iface = interface[interface.rfind(".") + 1:]
+	val = str(value)
+	print "{%s.%s} [%s] Path = %s" % (iface, member, path, val)
+
+if __name__ == '__main__':
+	dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+
+	bus = dbus.SystemBus()
+
+	bus.add_signal_receiver(property_changed, bus_name="org.bluez",
+					signal_name = "PropertyChanged",
+						path_keyword="path",
+						interface_keyword="interface")
+
+	bus.add_signal_receiver(object_signal, bus_name="org.bluez",
+					signal_name = "AdapterAdded",
+						path_keyword="path",
+						member_keyword="member",
+						interface_keyword="interface")
+	bus.add_signal_receiver(object_signal, bus_name="org.bluez",
+					signal_name = "AdapterRemoved",
+						path_keyword="path",
+						member_keyword="member",
+						interface_keyword="interface")
+	bus.add_signal_receiver(object_signal, bus_name="org.bluez",
+					signal_name = "DefaultAdapterChanged",
+						path_keyword="path",
+						member_keyword="member",
+						interface_keyword="interface")
+
+	bus.add_signal_receiver(object_signal, bus_name="org.bluez",
+					signal_name = "DeviceCreated",
+						path_keyword="path",
+						member_keyword="member",
+						interface_keyword="interface")
+	bus.add_signal_receiver(object_signal, bus_name="org.bluez",
+					signal_name = "DeviceRemoved",
+						path_keyword="path",
+						member_keyword="member",
+						interface_keyword="interface")
+
+	mainloop = gobject.MainLoop()
+	mainloop.run()
diff --git a/test/rctest.1 b/test/rctest.1
new file mode 100644
index 0000000..dfedbef
--- /dev/null
+++ b/test/rctest.1
@@ -0,0 +1,90 @@
+.TH RCTEST 1 "Jul 6 2009" BlueZ ""
+.SH NAME
+rctest \- RFCOMM testing
+.SH SYNOPSIS
+.B rctest
+<\fImode\fR> [\fIoptions\fR] [\fIbdaddr\fR]
+
+.SH DESCRIPTION
+.LP
+.B
+rctest
+is used to test RFCOMM communications on the BlueZ stack
+
+.SH MODES
+.TP
+.B -r
+listen and receive
+.TP
+.B -w
+listen and send
+.TP
+.B -d
+listen and dump incoming data
+.TP
+.B -s
+connect and send
+.TP
+.B -u
+connect and receive
+.TP
+.B -n
+connect and be silent
+.TP
+.B -c
+connect, disconnect, connect, ...
+.TP
+.B -m
+multiple connects
+
+.SH OPTIONS
+.TP
+.BI -b\  bytes
+send/receive \fIbytes\fR bytes
+.TP
+.BI -i\  device
+select the specified \fIdevice\fR
+.TP
+.BI -P\  channel
+select the specified \fIchannel\fR
+.TP
+.BI -U\  uuid
+select the specified \fIuuid\fR
+.TP
+.BI -L\  seconds
+enable SO_LINGER options for \fIseconds\fR
+.TP
+.BI -W\  seconds
+enable deferred setup for \fIseconds\fR
+.TP
+.BI -B\  filename
+use data packets from \fIfilename\fR
+.TP
+.BI -N\  num
+send \fInum\fR frames
+.TP
+.BI -C\  num
+send \fInum\fR frames before delay (default: 1)
+.TP
+.BI -D\  milliseconds
+delay \fImilliseconds\fR after sending \fInum\fR frames (default: 0)
+.TP
+.B -A
+request authentication
+.TP
+.B -E
+request encryption
+.TP
+.B -S
+secure connection
+.TP
+.B -M
+become master
+.TP
+.B -T
+enable timestamps
+
+.SH AUTHORS
+Written by Marcel Holtmann <marcel@holtmann.org> and Maxim Krasnyansky
+<maxk@qualcomm.com>, man page by Filippo Giunchedi <filippo@debian.org>
+.PP
diff --git a/test/rctest.c b/test/rctest.c
new file mode 100644
index 0000000..b3804f5
--- /dev/null
+++ b/test/rctest.c
@@ -0,0 +1,781 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <ctype.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <syslog.h>
+#include <signal.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/rfcomm.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+/* Test modes */
+enum {
+	SEND,
+	RECV,
+	RECONNECT,
+	MULTY,
+	DUMP,
+	CONNECT,
+	CRECV,
+	LSEND
+};
+
+static unsigned char *buf;
+
+/* Default data size */
+static long data_size = 127;
+static long num_frames = -1;
+
+/* Default number of consecutive frames before the delay */
+static int count = 1;
+
+/* Default delay after sending count number of frames */
+static unsigned long delay = 0;
+
+/* Default addr and channel */
+static bdaddr_t bdaddr;
+static uint16_t uuid = 0x0000;
+static uint8_t channel = 10;
+
+static char *filename = NULL;
+
+static int master = 0;
+static int auth = 0;
+static int encrypt = 0;
+static int secure = 0;
+static int socktype = SOCK_STREAM;
+static int linger = 0;
+static int timestamp = 0;
+static int defer_setup = 0;
+
+static float tv2fl(struct timeval tv)
+{
+	return (float)tv.tv_sec + (float)(tv.tv_usec/1000000.0);
+}
+
+static uint8_t get_channel(const char *svr, uint16_t uuid)
+{
+	sdp_session_t *sdp;
+	sdp_list_t *srch, *attrs, *rsp;
+	uuid_t svclass;
+	uint16_t attr;
+	bdaddr_t dst;
+	uint8_t channel = 0;
+	int err;
+
+	str2ba(svr, &dst);
+
+	sdp = sdp_connect(&bdaddr, &dst, SDP_RETRY_IF_BUSY);
+	if (!sdp)
+		return 0;
+
+	sdp_uuid16_create(&svclass, uuid);
+	srch = sdp_list_append(NULL, &svclass);
+
+	attr = SDP_ATTR_PROTO_DESC_LIST;
+	attrs = sdp_list_append(NULL, &attr);
+
+	err = sdp_service_search_attr_req(sdp, srch,
+					SDP_ATTR_REQ_INDIVIDUAL, attrs, &rsp);
+	if (err)
+		goto done;
+
+	for (; rsp; rsp = rsp->next) {
+		sdp_record_t *rec = (sdp_record_t *) rsp->data;
+		sdp_list_t *protos;
+
+		if (!sdp_get_access_protos(rec, &protos)) {
+			channel = sdp_get_proto_port(protos, RFCOMM_UUID);
+			if (channel > 0)
+				break;
+		}
+	}
+
+done:
+	sdp_close(sdp);
+
+	return channel;
+}
+
+static int do_connect(const char *svr)
+{
+	struct sockaddr_rc addr;
+	struct rfcomm_conninfo conn;
+	socklen_t optlen;
+	int sk, opt;
+
+	if (uuid != 0x0000)
+		channel = get_channel(svr, uuid);
+
+	if (channel == 0) {
+		syslog(LOG_ERR, "Can't get channel number");
+		return -1;
+	}
+
+	/* Create socket */
+	sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
+	if (sk < 0) {
+		syslog(LOG_ERR, "Can't create socket: %s (%d)",
+							strerror(errno), errno);
+		return -1;
+	}
+
+	/* Bind to local address */
+	memset(&addr, 0, sizeof(addr));
+	addr.rc_family = AF_BLUETOOTH;
+	bacpy(&addr.rc_bdaddr, &bdaddr);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+#if 0
+	/* Enable SO_TIMESTAMP */
+	if (timestamp) {
+		int t = 1;
+
+		if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
+			syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
+							strerror(errno), errno);
+			goto error;
+		}
+	}
+#endif
+
+	/* Enable SO_LINGER */
+	if (linger) {
+		struct linger l = { .l_onoff = 1, .l_linger = linger };
+
+		if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
+			syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
+							strerror(errno), errno);
+			goto error;
+		}
+	}
+
+	/* Set link mode */
+	opt = 0;
+	if (master)
+		opt |= RFCOMM_LM_MASTER;
+	if (auth)
+		opt |= RFCOMM_LM_AUTH;
+	if (encrypt)
+		opt |= RFCOMM_LM_ENCRYPT;
+	if (secure)
+		opt |= RFCOMM_LM_SECURE;
+
+	if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
+		syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Connect to remote device */
+	memset(&addr, 0, sizeof(addr));
+	addr.rc_family = AF_BLUETOOTH;
+	str2ba(svr, &addr.rc_bdaddr);
+	addr.rc_channel = channel;
+
+	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		syslog(LOG_ERR, "Can't connect: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Get connection information */
+	memset(&conn, 0, sizeof(conn));
+	optlen = sizeof(conn);
+
+	if (getsockopt(sk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
+							strerror(errno), errno);
+		//goto error;
+	}
+
+	syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x]",
+		conn.hci_handle,
+		conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
+
+	return sk;
+
+error:
+	close(sk);
+	return -1;
+}
+
+static void do_listen(void (*handler)(int sk))
+{
+	struct sockaddr_rc addr;
+	struct rfcomm_conninfo conn;
+	socklen_t optlen;
+	int sk, nsk, opt;
+	char ba[18];
+
+	/* Create socket */
+	sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
+	if (sk < 0) {
+		syslog(LOG_ERR, "Can't create socket: %s (%d)",
+							strerror(errno), errno);
+		exit(1);
+	}
+
+	/* Bind to local address */
+	memset(&addr, 0, sizeof(addr));
+	addr.rc_family = AF_BLUETOOTH;
+	bacpy(&addr.rc_bdaddr, &bdaddr);
+	addr.rc_channel = channel;
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Set link mode */
+	opt = 0;
+	if (master)
+		opt |= RFCOMM_LM_MASTER;
+	if (auth)
+		opt |= RFCOMM_LM_AUTH;
+	if (encrypt)
+		opt |= RFCOMM_LM_ENCRYPT;
+	if (secure)
+		opt |= RFCOMM_LM_SECURE;
+
+	if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
+		syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Enable deferred setup */
+	opt = defer_setup;
+
+	if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
+						&opt, sizeof(opt)) < 0) {
+		syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Listen for connections */
+	if (listen(sk, 10)) {
+		syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Check for socket address */
+	memset(&addr, 0, sizeof(addr));
+	optlen = sizeof(addr);
+
+	if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get socket name: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	channel = addr.rc_channel;
+
+	syslog(LOG_INFO, "Waiting for connection on channel %d ...", channel);
+
+	while (1) {
+		memset(&addr, 0, sizeof(addr));
+		optlen = sizeof(addr);
+
+		nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
+		if (nsk < 0) {
+			syslog(LOG_ERR,"Accept failed: %s (%d)",
+							strerror(errno), errno);
+			goto error;
+		}
+		if (fork()) {
+			/* Parent */
+			close(nsk);
+			continue;
+		}
+		/* Child */
+		close(sk);
+
+		/* Get connection information */
+		memset(&conn, 0, sizeof(conn));
+		optlen = sizeof(conn);
+
+		if (getsockopt(nsk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
+			syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
+							strerror(errno), errno);
+			//close(nsk);
+			//goto error;
+		}
+
+		ba2str(&addr.rc_bdaddr, ba);
+		syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]",
+			ba, conn.hci_handle,
+			conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
+
+#if 0
+		/* Enable SO_TIMESTAMP */
+		if (timestamp) {
+			int t = 1;
+
+			if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
+				syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
+							strerror(errno), errno);
+				goto error;
+			}
+		}
+#endif
+
+		/* Enable SO_LINGER */
+		if (linger) {
+			struct linger l = { .l_onoff = 1, .l_linger = linger };
+
+			if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
+				syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
+							strerror(errno), errno);
+				close(nsk);
+				goto error;
+			}
+		}
+
+		/* Handle deferred setup */
+		if (defer_setup) {
+			syslog(LOG_INFO, "Waiting for %d seconds",
+							abs(defer_setup) - 1);
+			sleep(abs(defer_setup) - 1);
+
+			if (defer_setup < 0) {
+				close(nsk);
+				goto error;
+			}
+		}
+
+		handler(nsk);
+
+		syslog(LOG_INFO, "Disconnect: %m");
+		exit(0);
+	}
+
+	return;
+
+error:
+	close(sk);
+	exit(1);
+}
+
+static void dump_mode(int sk)
+{
+	int len;
+
+	syslog(LOG_INFO, "Receiving ...");
+	while ((len = read(sk, buf, data_size)) > 0)
+		syslog(LOG_INFO, "Recevied %d bytes", len);
+}
+
+static void recv_mode(int sk)
+{
+	struct timeval tv_beg, tv_end, tv_diff;
+	char ts[30];
+	long total;
+	uint32_t seq;
+
+	syslog(LOG_INFO, "Receiving ...");
+
+	memset(ts, 0, sizeof(ts));
+
+	seq = 0;
+	while (1) {
+		gettimeofday(&tv_beg,NULL);
+		total = 0;
+		while (total < data_size) {
+			//uint32_t sq;
+			//uint16_t l;
+			int r;
+
+			if ((r = recv(sk, buf, data_size, 0)) < 0) {
+				if (r < 0)
+					syslog(LOG_ERR, "Read failed: %s (%d)",
+							strerror(errno), errno);
+				return;
+			}
+
+			if (timestamp) {
+				struct timeval tv;
+
+				if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
+					timestamp = 0;
+					memset(ts, 0, sizeof(ts));
+				} else {
+					sprintf(ts, "[%ld.%ld] ",
+							tv.tv_sec, tv.tv_usec);
+				}
+			}
+
+#if 0
+			/* Check sequence */
+			sq = btohl(*(uint32_t *) buf);
+			if (seq != sq) {
+				syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
+				seq = sq;
+			}
+			seq++;
+
+			/* Check length */
+			l = btohs(*(uint16_t *) (buf + 4));
+			if (r != l) {
+				syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
+				continue;
+			}
+
+			/* Verify data */
+			for (i = 6; i < r; i++) {
+				if (buf[i] != 0x7f)
+					syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
+			}
+#endif
+			total += r;
+		}
+		gettimeofday(&tv_end,NULL);
+
+		timersub(&tv_end,&tv_beg,&tv_diff);
+
+		syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
+			tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
+	}
+}
+
+static void do_send(int sk)
+{
+	uint32_t seq;
+	int i, fd, len;
+
+	syslog(LOG_INFO,"Sending ...");
+
+	if (filename) {
+		fd = open(filename, O_RDONLY);
+		if (fd < 0) {
+			syslog(LOG_ERR, "Open failed: %s (%d)",
+							strerror(errno), errno);
+			exit(1);
+		}
+		len = read(fd, buf, data_size);
+		send(sk, buf, len, 0);
+		return;
+	} else {
+		for (i = 6; i < data_size; i++)
+			buf[i] = 0x7f;
+	}
+
+	seq = 0;
+	while ((num_frames == -1) || (num_frames-- > 0)) {
+		*(uint32_t *) buf = htobl(seq);
+		*(uint16_t *) (buf + 4) = htobs(data_size);
+		seq++;
+
+		if (send(sk, buf, data_size, 0) <= 0) {
+			syslog(LOG_ERR, "Send failed: %s (%d)",
+							strerror(errno), errno);
+			exit(1);
+		}
+
+		if (num_frames && delay && count && !(seq % count))
+			usleep(delay);
+	}
+}
+
+static void send_mode(int sk)
+{
+	do_send(sk);
+
+	syslog(LOG_INFO, "Closing channel ...");
+	if (shutdown(sk, SHUT_RDWR) < 0)
+		syslog(LOG_INFO, "Close failed: %m");
+	else
+		syslog(LOG_INFO, "Done");
+}
+
+static void reconnect_mode(char *svr)
+{
+	while(1) {
+		int sk = do_connect(svr);
+		close(sk);
+	}
+}
+
+static void multi_connect_mode(int argc, char *argv[])
+{
+	int i, n, sk;
+
+	while (1) {
+		for (n = 0; n < argc; n++) {
+			for (i = 0; i < count; i++) {
+				if (fork())
+					continue;
+
+				/* Child */
+				sk = do_connect(argv[n]);
+				usleep(500);
+				close(sk);
+				exit(0);
+			}
+		}
+		sleep(4);
+	}
+}
+
+static void usage(void)
+{
+	printf("rctest - RFCOMM testing\n"
+		"Usage:\n");
+	printf("\trctest <mode> [options] [bdaddr]\n");
+	printf("Modes:\n"
+		"\t-r listen and receive\n"
+		"\t-w listen and send\n"
+		"\t-d listen and dump incoming data\n"
+		"\t-s connect and send\n"
+		"\t-u connect and receive\n"
+		"\t-n connect and be silent\n"
+		"\t-c connect, disconnect, connect, ...\n"
+		"\t-m multiple connects\n");
+
+	printf("Options:\n"
+		"\t[-b bytes] [-i device] [-P channel] [-U uuid]\n"
+		"\t[-L seconds] enabled SO_LINGER option\n"
+		"\t[-W seconds] enable deferred setup\n"
+		"\t[-B filename] use data packets from file\n"
+		"\t[-N num] number of frames to send\n"
+		"\t[-C num] send num frames before delay (default = 1)\n"
+		"\t[-D milliseconds] delay after sending num frames (default = 0)\n"
+		"\t[-A] request authentication\n"
+		"\t[-E] request encryption\n"
+		"\t[-S] secure connection\n"
+		"\t[-M] become master\n"
+		"\t[-T] enable timestamps\n");
+}
+
+int main(int argc, char *argv[])
+{
+	struct sigaction sa;
+	int opt, sk, mode = RECV, need_addr = 0;
+
+	bacpy(&bdaddr, BDADDR_ANY);
+
+	while ((opt=getopt(argc,argv,"rdscuwmnb:i:P:U:B:N:MAESL:W:C:D:T")) != EOF) {
+		switch (opt) {
+		case 'r':
+			mode = RECV;
+			break;
+
+		case 's':
+			mode = SEND;
+			need_addr = 1;
+			break;
+
+		case 'w':
+			mode = LSEND;
+			break;
+
+		case 'u':
+			mode = CRECV;
+			need_addr = 1;
+			break;
+
+		case 'd':
+			mode = DUMP;
+			break;
+
+		case 'c':
+			mode = RECONNECT;
+			need_addr = 1;
+			break;
+
+		case 'n':
+			mode = CONNECT;
+			need_addr = 1;
+			break;
+
+		case 'm':
+			mode = MULTY;
+			need_addr = 1;
+			break;
+
+		case 'b':
+			data_size = atoi(optarg);
+			break;
+
+		case 'i':
+			if (!strncasecmp(optarg, "hci", 3))
+				hci_devba(atoi(optarg + 3), &bdaddr);
+			else
+				str2ba(optarg, &bdaddr);
+			break;
+
+		case 'P':
+			channel = atoi(optarg);
+			break;
+
+		case 'U':
+			if (!strcasecmp(optarg, "spp"))
+				uuid = SERIAL_PORT_SVCLASS_ID;
+			else if (!strncasecmp(optarg, "0x", 2))
+				uuid = strtoul(optarg + 2, NULL, 16);
+			else
+				uuid = atoi(optarg);
+			break;
+
+		case 'M':
+			master = 1;
+			break;
+
+		case 'A':
+			auth = 1;
+			break;
+
+		case 'E':
+			encrypt = 1;
+			break;
+
+		case 'S':
+			secure = 1;
+			break;
+
+		case 'L':
+			linger = atoi(optarg);
+			break;
+
+		case 'W':
+			defer_setup = atoi(optarg);
+			break;
+
+		case 'B':
+			filename = strdup(optarg);
+			break;
+
+		case 'N':
+			num_frames = atoi(optarg);
+			break;
+
+		case 'C':
+			count = atoi(optarg);
+			break;
+
+		case 'D':
+			delay = atoi(optarg) * 1000;
+			break;
+
+		case 'T':
+			timestamp = 1;
+			break;
+
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	if (need_addr && !(argc - optind)) {
+		usage();
+		exit(1);
+	}
+
+	if (!(buf = malloc(data_size))) {
+		perror("Can't allocate data buffer");
+		exit(1);
+	}
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_handler = SIG_IGN;
+	sa.sa_flags   = SA_NOCLDSTOP;
+	sigaction(SIGCHLD, &sa, NULL);
+
+	openlog("rctest", LOG_PERROR | LOG_PID, LOG_LOCAL0);
+
+	switch (mode) {
+		case RECV:
+			do_listen(recv_mode);
+			break;
+
+		case CRECV:
+			sk = do_connect(argv[optind]);
+			if (sk < 0)
+				exit(1);
+			recv_mode(sk);
+			break;
+
+		case DUMP:
+			do_listen(dump_mode);
+			break;
+
+		case SEND:
+			sk = do_connect(argv[optind]);
+			if (sk < 0)
+				exit(1);
+			send_mode(sk);
+			break;
+
+		case LSEND:
+			do_listen(send_mode);
+			break;
+
+		case RECONNECT:
+			reconnect_mode(argv[optind]);
+			break;
+
+		case MULTY:
+			multi_connect_mode(argc - optind, argv + optind);
+			break;
+
+		case CONNECT:
+			sk = do_connect(argv[optind]);
+			if (sk < 0)
+				exit(1);
+			dump_mode(sk);
+			break;
+	}
+
+	syslog(LOG_INFO, "Exit");
+
+	closelog();
+
+	return 0;
+}
diff --git a/test/sap-client b/test/sap-client
new file mode 100644
index 0000000..b12d455
--- /dev/null
+++ b/test/sap-client
@@ -0,0 +1,943 @@
+""" Copyright (C) 2010-2011 ST-Ericsson SA """
+
+""" Author: Szymon Janc <szymon.janc@tieto.com> for ST-Ericsson. """
+
+""" This program is free software; you can redistribute it and/or modify """
+""" it under the terms of the GNU General Public License as published by """
+""" the Free Software Foundation; either version 2 of the License, or """
+""" (at your option) any later version. """
+
+""" This program is distributed in the hope that it will be useful, """
+""" but WITHOUT ANY WARRANTY; without even the implied warranty of """
+""" MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the """
+""" GNU General Public License for more details. """
+
+""" You should have received a copy of the GNU General Public License """
+""" along with this program; if not, write to the Free Software """
+""" Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA """
+
+from array import array
+from bluetooth import *
+import time
+import re
+
+class SAPParam:
+    """ SAP Parameter Class """
+
+    MaxMsgSize = 0x00
+    ConnectionStatus = 0x01
+    ResultCode = 0x02
+    DisconnectionType = 0x03
+    CommandAPDU = 0x04
+    ResponseAPDU = 0x05
+    ATR = 0x06
+    CardReaderStatus = 0x07
+    StatusChange = 0x08
+    TransportProtocol = 0x09
+    CommandAPDU7816 = 0x10
+
+    def __init__(self, name, id, value = None):
+        self.name = name
+        self.id = id
+        self.value = value
+
+    def _padding(self,  buf):
+        pad = array('B')
+        while ( (len(buf) + len(pad)) % 4 ) != 0:
+            pad.append(0)
+        return pad
+
+    def _basicCheck(self,  buf):
+        if len(buf) < 4 or (len(buf) % 4) != 0 or buf[1] != 0:
+                return (-1,  -1)
+        if buf[0] != self.id:
+            return (-1,  -1)
+        plen = buf[2] * 256 + buf[3] + 4
+        if plen > len(buf):
+            return (-1,  -1)
+        pad = plen
+        while (pad % 4) != 0:
+            if buf[pad] != 0:
+                return (-1,  -1)
+            pad+=1
+        return (plen,  pad)
+
+    def getID(self):
+        return self.id
+
+    def getValue(self):
+        return self.value
+
+    def getContent(self):
+        return "%s(id=0x%.2X), value=%s \n" %  (self.name,  self.id, self.value)
+
+    def serialize(self):
+        a = array('B', '\00\00\00\00')
+        a[0] = self.id
+        a[1] = 0	# reserved
+        a[2] = 0	# length
+        a[3] = 1	# length
+        a.append(self.value)
+        a.extend(self._padding(a))
+        return a
+
+    def deserialize(self,  buf):
+        p = self._basicCheck(buf)
+        if p[0] == -1:
+            return -1
+        self.id = buf[0]
+        self.value = buf[4]
+        return p[1]
+
+
+class SAPParam_MaxMsgSize(SAPParam):
+    """MaxMsgSize Param """
+
+    def __init__(self,  value = None):
+        SAPParam.__init__(self,"MaxMsgSize",  SAPParam.MaxMsgSize, value)
+        self.__validate()
+
+    def __validate(self):
+        if self.value > 0xFFFF:
+             self.value = 0xFFFF
+
+    def serialize(self):
+        a = array('B', '\00\00\00\00')
+        a[0] = self.id
+        a[3] = 2
+        a.append(self.value / 256)
+        a.append(self.value % 256)
+        a.extend(self._padding(a))
+        return a
+
+    def deserialize(self,  buf):
+        p = self._basicCheck(buf)
+        if p[0] == -1 :
+            return -1
+        self.value = buf[4] * 256 + buf[5]
+        return p[1]
+
+class SAPParam_CommandAPDU(SAPParam):
+    def __init__(self,  value = None):
+        if value is None:
+            SAPParam.__init__(self, "CommandAPDU",  SAPParam.CommandAPDU, array('B'))
+        else:
+            SAPParam.__init__(self, "CommandAPDU",  SAPParam.CommandAPDU, array('B', value))
+
+    def serialize(self):
+        a = array('B', '\00\00\00\00')
+        a[0] = self.id
+        plen = len(self.value)
+        a[2] = plen / 256
+        a[3] = plen % 256
+        a.extend(self.value)
+        a.extend(self._padding(a))
+        return a
+
+    def deserialize(self,  buf):
+        p = self._basicCheck(buf)
+        if p[0] == -1:
+            return -1
+        self.value = buf[4:p[0]]
+        return p[1]
+
+class SAPParam_ResponseAPDU(SAPParam_CommandAPDU):
+    """ResponseAPDU Param """
+
+    def __init__(self,  value = None):
+        if value is None:
+            SAPParam.__init__(self, "ResponseAPDU",  SAPParam.ResponseAPDU, array('B'))
+        else:
+            SAPParam.__init__(self, "ResponseAPDU",  SAPParam.ResponseAPDU, array('B', value))
+
+class SAPParam_ATR(SAPParam_CommandAPDU):
+    """ATR Param """
+
+    def __init__(self,  value = None):
+        if value is None:
+            SAPParam.__init__(self, "ATR",  SAPParam.ATR, array('B'))
+        else:
+            SAPParam.__init__(self, "ATR",  SAPParam.ATR, array('B', value))
+
+class SAPParam_CommandAPDU7816(SAPParam_CommandAPDU):
+    """Command APDU7816 Param."""
+
+    def __init__(self,  value = None):
+        if value is None:
+            SAPParam.__init__(self, "CommandAPDU7816",  SAPParam.CommandAPDU7816, array('B'))
+        else:
+            SAPParam.__init__(self, "CommandAPDU7816",  SAPParam.CommandAPDU7816, array('B', value))
+
+
+class SAPParam_ConnectionStatus(SAPParam):
+    """Connection status Param."""
+
+    def __init__(self,  value = None):
+        SAPParam.__init__(self,"ConnectionStatus",  SAPParam.ConnectionStatus, value)
+        self.__validate()
+
+    def __validate(self):
+        if self.value is not None and self.value not in (0x00,  0x01,  0x02,  0x03,  0x04):
+            print "Warning. ConnectionStatus value in reserved range (0x%x)" % self.value
+
+    def deserialize(self,  buf):
+        ret = SAPParam.deserialize(self, buf)
+        if ret == -1:
+            return -1
+        self.__validate()
+        return ret
+
+class SAPParam_ResultCode(SAPParam):
+    """ Result Code Param """
+
+    def __init__(self,  value = None):
+        SAPParam.__init__(self,"ResultCode",  SAPParam.ResultCode, value)
+        self.__validate()
+
+    def __validate(self):
+        if self.value is not None and self.value not in (0x00,  0x01,  0x02,  0x03,  0x04,  0x05,  0x06,  0x07):
+            print "Warning. ResultCode value in reserved range (0x%x)" % self.value
+
+    def deserialize(self,  buf):
+        ret = SAPParam.deserialize(self, buf)
+        if ret == -1:
+            return -1
+        self.__validate()
+        return ret
+
+class SAPParam_DisconnectionType(SAPParam):
+    """Disconnection Type Param."""
+
+    def __init__(self,  value = None):
+        SAPParam.__init__(self,"DisconnectionType",  SAPParam.DisconnectionType, value)
+        self.__validate()
+
+    def __validate(self):
+        if self.value is not None and self.value not in (0x00,  0x01):
+            print "Warning. DisconnectionType value in reserved range (0x%x)" % self.value
+
+    def deserialize(self,  buf):
+        ret = SAPParam.deserialize(self, buf)
+        if ret == -1:
+            return -1
+        self.__validate()
+        return ret
+
+class SAPParam_CardReaderStatus(SAPParam_CommandAPDU):
+    """Card reader Status Param."""
+
+    def __init__(self,  value = None):
+        if value is None:
+            SAPParam.__init__(self, "CardReaderStatus",  SAPParam.CardReaderStatus, array('B'))
+        else:
+            SAPParam.__init__(self, "CardReaderStatus",  SAPParam.CardReaderStatus, array('B', value))
+
+class SAPParam_StatusChange(SAPParam):
+    """Status Change Param """
+
+    def __init__(self,  value = None):
+        SAPParam.__init__(self,"StatusChange",  SAPParam.StatusChange, value)
+
+    def __validate(self):
+        if self.value is not None and self.value not in (0x00,  0x01,  0x02,  0x03,  0x04,  0x05):
+            print "Warning. StatusChange value in reserved range (0x%x)" % self.value
+
+    def deserialize(self,  buf):
+        ret = SAPParam.deserialize(self, buf)
+        if ret == -1:
+            return -1
+        self.__validate()
+        return ret
+
+class SAPParam_TransportProtocol(SAPParam):
+    """Transport Protocol Param """
+
+    def __init__(self,  value = None):
+        SAPParam.__init__(self,"TransportProtocol",  SAPParam.TransportProtocol, value)
+        self.__validate()
+
+    def __validate(self):
+        if self.value is not None and self.value not in (0x00,  0x01):
+            print "Warning. TransportProtoco value in reserved range (0x%x)" % self.value
+
+    def deserialize(self,  buf):
+        ret = SAPParam.deserialize(self, buf)
+        if ret == -1:
+            return -1
+        self.__validate()
+        return ret
+
+class SAPMessage:
+
+    CONNECT_REQ = 0x00
+    CONNECT_RESP = 0x01
+    DISCONNECT_REQ = 0x02
+    DISCONNECT_RESP =0x03
+    DISCONNECT_IND = 0x04
+    TRANSFER_APDU_REQ = 0x05
+    TRANSFER_APDU_RESP = 0x06
+    TRANSFER_ATR_REQ = 0x07
+    TRANSFER_ATR_RESP = 0x08
+    POWER_SIM_OFF_REQ = 0x09
+    POWER_SIM_OFF_RESP = 0x0A
+    POWER_SIM_ON_REQ = 0x0B
+    POWER_SIM_ON_RESP = 0x0C
+    RESET_SIM_REQ = 0x0D
+    RESET_SIM_RESP = 0x0E
+    TRANSFER_CARD_READER_STATUS_REQ = 0x0F
+    TRANSFER_CARD_READER_STATUS_RESP = 0x10
+    STATUS_IND = 0x11
+    ERROR_RESP = 0x12
+    SET_TRANSPORT_PROTOCOL_REQ = 0x13
+    SET_TRANSPORT_PROTOCOL_RESP = 0x14
+
+    def __init__(self,  name,  id):
+        self.name = name
+        self.id = id
+        self.params = []
+        self.buf = array('B')
+
+    def _basicCheck(self,  buf):
+        if len(buf) < 4 or (len(buf) % 4) != 0 :
+            return False
+
+        if buf[0] != self.id:
+            return False
+
+        return True
+
+    def getID(self):
+        return self.id
+
+    def getContent(self):
+        s = "%s(id=0x%.2X) " % (self.name,  self.id)
+        if len( self.buf): s = s + "[%s]" % re.sub("(.{2})", "0x\\1 " , self.buf.tostring().encode("hex").upper(), re.DOTALL)
+        s = s + "\n\t"
+        for p in self.params:
+            s = s + "\t" + p.getContent()
+        return s
+
+    def getParams(self):
+        return self.params
+
+    def addParam(self,  param):
+        self.params.append(param)
+
+    def serialize(self):
+        ret = array('B', '\00\00\00\00')
+        ret[0] = self.id
+        ret[1] = len(self.params)
+        ret[2] = 0	# reserved
+        ret[3] = 0	# reserved
+        for p in self.params:
+            ret.extend(p.serialize())
+
+        self.buf = ret
+        return ret
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        return len(buf) == 4 and buf[1] == 0 and self._basicCheck(buf)
+
+
+class SAPMessage_CONNECT_REQ(SAPMessage):
+    def __init__(self,  MaxMsgSize = None):
+        SAPMessage.__init__(self,"CONNECT_REQ",  SAPMessage.CONNECT_REQ)
+        if MaxMsgSize is not None:
+            self.addParam(SAPParam_MaxMsgSize(MaxMsgSize))
+
+    def _validate(self):
+        if len(self.params) == 1:
+            if self.params[0].getID() == SAPParam.MaxMsgSize:
+                return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+        if SAPMessage._basicCheck(self,  buf):
+            p = SAPParam_MaxMsgSize()
+            if p.deserialize(buf[4:]) == len(buf[4:]):
+                self.addParam(p)
+                return self._validate()
+
+        return False
+
+class SAPMessage_CONNECT_RESP(SAPMessage):
+    def __init__(self,  ConnectionStatus = None,  MaxMsgSize = None):
+        SAPMessage.__init__(self,"CONNECT_RESP",  SAPMessage.CONNECT_RESP)
+        if ConnectionStatus is not None:
+            self.addParam(SAPParam_ConnectionStatus(ConnectionStatus))
+            if MaxMsgSize is not None:
+                self.addParam(SAPParam_MaxMsgSize(MaxMsgSize))
+
+    def _validate(self):
+        if len(self.params) > 0:
+            if self.params[0] .getID() == SAPParam.ConnectionStatus:
+                if self.params[0].getValue() ==  0x02:
+                    if len(self.params) == 2:
+                        return True
+                else:
+                    if len(self.params) == 1:
+                        return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+
+        if SAPMessage._basicCheck(self,  buf):
+            p = SAPParam_ConnectionStatus()
+            r = p.deserialize(buf[4:])
+            if  r != -1:
+                self.addParam(p)
+                if buf[1] == 2:
+                    p = SAPParam_MaxMsgSize()
+                    r = p.deserialize(buf[4+r:])
+                    if r != -1:
+                        self.addParam(p)
+
+                return self._validate()
+
+        return False
+
+class SAPMessage_DISCONNECT_REQ(SAPMessage):
+    def __init__(self):
+        SAPMessage.__init__(self,"DISCONNECT_REQ",  SAPMessage.DISCONNECT_REQ)
+
+class SAPMessage_DISCONNECT_RESP(SAPMessage):
+    def __init__(self):
+        SAPMessage.__init__(self,"DISCONNECT_RESP",  SAPMessage.DISCONNECT_RESP)
+
+class SAPMessage_DISCONNECT_IND(SAPMessage):
+    def __init__(self,  Type = None):
+        SAPMessage.__init__(self,"DISCONNECT_IND",  SAPMessage.DISCONNECT_IND)
+        if Type is not None:
+            self.addParam(SAPParam_DisconnectionType(Type))
+
+    def _validate(self):
+        if len(self.params) == 1:
+            if self.params[0].getID() == SAPParam.DisconnectionType:
+                return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+        if SAPMessage._basicCheck(self,  buf):
+            p = SAPParam_DisconnectionType()
+            if p.deserialize(buf[4:]) == len(buf[4:]):
+                self.addParam(p)
+                return self._validate()
+
+        return False
+
+
+class SAPMessage_TRANSFER_APDU_REQ(SAPMessage):
+    def __init__(self,  APDU = None,  T = False):
+        SAPMessage.__init__(self,"TRANSFER_APDU_REQ",  SAPMessage.TRANSFER_APDU_REQ)
+        if APDU is not None:
+            if T :
+                self.addParam(SAPParam_CommandAPDU(APDU))
+            else:
+                self.addParam(SAPParam_CommandAPDU7816(APDU))
+
+    def _validate(self):
+        if len(self.params) == 1:
+            if self.params[0].getID() == SAPParam.CommandAPDU or self.params[0].getID() == SAPParam.CommandAPDU7816:
+                return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+        if SAPMessage._basicCheck(self,  buf):
+
+            p = SAPParam_CommandAPDU()
+            p2 = SAPParam_CommandAPDU7816()
+            if p.deserialize(buf[4:]) == len(buf[4:]):
+                self.addParam(p)
+                return self._validate()
+            elif p2.deserialize(buf[4:]) == len(buf[4:]):
+                self.addParam(p2)
+                return self._validate()
+
+        return False
+
+class SAPMessage_TRANSFER_APDU_RESP(SAPMessage):
+    def __init__(self,  ResultCode = None,  Response = None):
+        SAPMessage.__init__(self,"TRANSFER_APDU_RESP",  SAPMessage.TRANSFER_APDU_RESP)
+        if ResultCode is not None:
+            self.addParam(SAPParam_ResultCode(ResultCode))
+            if Response is not None:
+                self.addParam(SAPParam_ResponseAPDU(Response))
+
+    def _validate(self):
+        if len(self.params) > 0:
+            if self.params[0] .getID() == SAPParam.ResultCode:
+                if self.params[0].getValue() == 0x00:
+                    if len(self.params) == 2:
+                        return True
+                else:
+                    if len(self.params) == 1:
+                        return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+
+        if SAPMessage._basicCheck(self,  buf):
+            p = SAPParam_ResultCode()
+            r = p.deserialize(buf[4:])
+            if  r != -1:
+                self.addParam(p)
+                if buf[1] == 2:
+                    p = SAPParam_ResponseAPDU()
+                    r = p.deserialize(buf[4+r:])
+                    if r != -1:
+                        self.addParam(p)
+
+                return self._validate()
+
+        return False
+
+class SAPMessage_TRANSFER_ATR_REQ(SAPMessage):
+    def __init__(self):
+        SAPMessage.__init__(self,"TRANSFER_ATR_REQ",  SAPMessage.TRANSFER_ATR_REQ)
+
+class SAPMessage_TRANSFER_ATR_RESP(SAPMessage):
+    def __init__(self,  ResultCode = None,  ATR = None):
+        SAPMessage.__init__(self,"TRANSFER_ATR_RESP",  SAPMessage.TRANSFER_ATR_RESP)
+        if ResultCode is not None:
+            self.addParam(SAPParam_ResultCode(ResultCode))
+            if ATR is not None:
+                self.addParam(SAPParam_ATR(ATR))
+
+    def _validate(self):
+        if len(self.params) > 0:
+            if self.params[0] .getID() == SAPParam.ResultCode:
+                if self.params[0].getValue() == 0x00:
+                    if len(self.params) == 2:
+                        return True
+                else:
+                    if len(self.params) == 1:
+                        return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+
+        if SAPMessage._basicCheck(self,  buf):
+
+            p = SAPParam_ResultCode()
+            r = p.deserialize(buf[4:])
+
+            if  r != -1:
+
+                self.addParam(p)
+                if buf[1] == 2:
+
+                    p = SAPParam_ATR()
+                    r = p.deserialize(buf[4+r:])
+                    if r != -1:
+                        self.addParam(p)
+
+                return self._validate()
+
+        return False
+
+class SAPMessage_POWER_SIM_OFF_REQ(SAPMessage):
+    def __init__(self):
+        SAPMessage.__init__(self,"POWER_SIM_OFF_REQ",  SAPMessage.POWER_SIM_OFF_REQ)
+
+class SAPMessage_POWER_SIM_OFF_RESP(SAPMessage):
+    def __init__(self,  ResultCode = None):
+        SAPMessage.__init__(self,"POWER_SIM_OFF_RESP",  SAPMessage.POWER_SIM_OFF_RESP)
+        if ResultCode is not None:
+            self.addParam(SAPParam_ResultCode(ResultCode))
+
+    def _validate(self):
+        if len(self.params) == 1:
+            if self.params[0].getID() == SAPParam.ResultCode:
+                return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+        if SAPMessage._basicCheck(self,  buf):
+            p = SAPParam_ResultCode()
+            if p.deserialize(buf[4:]) == len(buf[4:]):
+                self.addParam(p)
+                return self._validate()
+
+        return False
+
+class SAPMessage_POWER_SIM_ON_REQ(SAPMessage):
+    def __init__(self):
+        SAPMessage.__init__(self,"POWER_SIM_ON_REQ",  SAPMessage.POWER_SIM_ON_REQ)
+
+class SAPMessage_POWER_SIM_ON_RESP(SAPMessage_POWER_SIM_OFF_RESP):
+    def __init__(self,  ResultCode = None):
+        SAPMessage.__init__(self,"POWER_SIM_ON_RESP",  SAPMessage.POWER_SIM_ON_RESP)
+        if ResultCode is not None:
+            self.addParam(SAPParam_ResultCode(ResultCode))
+
+class SAPMessage_RESET_SIM_REQ(SAPMessage):
+    def __init__(self):
+        SAPMessage.__init__(self,"RESET_SIM_REQ",  SAPMessage.RESET_SIM_REQ)
+
+class SAPMessage_RESET_SIM_RESP(SAPMessage_POWER_SIM_OFF_RESP):
+    def __init__(self,  ResultCode = None):
+        SAPMessage.__init__(self,"RESET_SIM_RESP",  SAPMessage.RESET_SIM_RESP)
+        if ResultCode is not None:
+            self.addParam(SAPParam_ResultCode(ResultCode))
+
+class SAPMessage_STATUS_IND(SAPMessage):
+    def __init__(self,  StatusChange = None):
+        SAPMessage.__init__(self,"STATUS_IND",  SAPMessage.STATUS_IND)
+        if StatusChange is not None:
+            self.addParam(SAPParam_StatusChange(StatusChange))
+
+    def _validate(self):
+        if len(self.params) == 1:
+            if self.params[0].getID() == SAPParam.StatusChange:
+                return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+        if SAPMessage._basicCheck(self,  buf):
+            p = SAPParam_StatusChange()
+            if p.deserialize(buf[4:]) == len(buf[4:]):
+                self.addParam(p)
+                return self._validate()
+
+        return False
+
+class SAPMessage_TRANSFER_CARD_READER_STATUS_REQ(SAPMessage):
+    def __init__(self):
+        SAPMessage.__init__(self,"TRANSFER_CARD_READER_STATUS_REQ",  SAPMessage.TRANSFER_CARD_READER_STATUS_REQ)
+
+class SAPMessage_TRANSFER_CARD_READER_STATUS_RESP(SAPMessage):
+    def __init__(self,  ResultCode = None,  Status = None):
+        SAPMessage.__init__(self,"TRANSFER_CARD_READER_STATUS_RESP",  SAPMessage.TRANSFER_CARD_READER_STATUS_RESP)
+        if ResultCode is not None:
+            self.addParam(SAPParam_ResultCode(ResultCode))
+            if Status is not None:
+                self.addParam(SAPParam_CardReaderStatus(Status))
+
+    def _validate(self):
+        if len(self.params) > 0:
+            if self.params[0] .getID() == SAPParam.ResultCode:
+                if self.params[0].getValue() == 0x00:
+                    if len(self.params) == 2:
+                        return True
+                else:
+                    if len(self.params) == 1:
+                        return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+
+        if SAPMessage._basicCheck(self,  buf):
+            p = SAPParam_ResultCode()
+            r = p.deserialize(buf[4:])
+            if  r != -1:
+                self.addParam(p)
+                if buf[1] == 2:
+                    p = SAPParam_CardReaderStatus()
+                    r = p.deserialize(buf[4+r:])
+                    if r != -1:
+                        self.addParam(p)
+
+                return self._validate()
+
+        return False
+
+class SAPMessage_ERROR_RESP(SAPMessage):
+    def __init__(self):
+        SAPMessage.__init__(self,"ERROR_RESP",  SAPMessage.ERROR_RESP)
+
+
+class SAPMessage_SET_TRANSPORT_PROTOCOL_REQ(SAPMessage):
+    def __init__(self,  protocol = None):
+        SAPMessage.__init__(self,"SET_TRANSPORT_PROTOCOL_REQ",  SAPMessage.SET_TRANSPORT_PROTOCOL_REQ)
+        if protocol is not None:
+            self.addParam(SAPParam_TransportProtocol(protocol))
+
+    def _validate(self):
+        if len(self.params) == 1:
+            if self.params[0].getID() == SAPParam.TransportProtocol:
+                return True
+        return False
+
+    def deserialize(self,  buf):
+        self.buf = buf
+        self.params[:] = []
+        if SAPMessage._basicCheck(self,  buf):
+            p = SAPParam_TransportProtocol()
+            if p.deserialize(buf[4:]) == len(buf[4:]):
+                self.addParam(p)
+                return self._validate()
+
+        return False
+
+class SAPMessage_SET_TRANSPORT_PROTOCOL_RESP(SAPMessage_POWER_SIM_OFF_RESP):
+    def __init__(self,  ResultCode = None):
+        SAPMessage.__init__(self,"SET_TRANSPORT_PROTOCOL_RESP",  SAPMessage.SET_TRANSPORT_PROTOCOL_RESP)
+        if ResultCode is not None:
+            self.addParam(SAPParam_ResultCode(ResultCode))
+
+
+class SAPClient:
+
+    CONNECTED = 1
+    DISCONNECTED = 0
+
+    uuid = "0000112D-0000-1000-8000-00805F9B34FB"
+    bufsize = 1024
+    timeout = 20
+    state = DISCONNECTED
+
+    def __init__(self,  host = None,  port = None):
+        self.sock = None
+
+        if host is None or is_valid_address(host):
+            self.host = host
+        else:
+            raise BluetoothError ("%s is not a valid BT address." % host)
+            self.host = None
+            return
+
+        if port is None:
+            self.__discover()
+        else:
+            self.port = port
+
+        self.__connectRFCOMM()
+
+    def __del__(self):
+        self.__disconnectRFCOMM()
+
+    def __disconnectRFCOMM(self):
+        if self.sock is not None:
+            self.sock.close()
+            self.state = self.DISCONNECTED
+
+    def __discover(self):
+        service_matches = find_service(self.uuid, self.host)
+
+        if len(service_matches) == 0:
+            raise BluetoothError ("No SAP service found")
+            return
+
+        first_match = service_matches[0]
+        self.port = first_match["port"]
+        self.host = first_match["host"]
+
+        print "SAP Service found on %s(%s)" % first_match["name"] % self.host
+
+    def __connectRFCOMM(self):
+        self.sock=BluetoothSocket( RFCOMM )
+        self.sock.connect((self.host, self.port))
+        self.sock.settimeout(self.timeout)
+        self.state = self.CONNECTED
+
+    def __sendMsg(self, msg):
+        if isinstance(msg,  SAPMessage):
+            s = msg.serialize()
+            print "\tTX: " + msg.getContent()
+            return self.sock.send(s.tostring())
+
+    def __rcvMsg(self,  msg):
+        if isinstance(msg,  SAPMessage):
+            print "\tRX Wait: %s(id = 0x%.2x)" % (msg.name, msg.id)
+            data = self.sock.recv(self.bufsize)
+            if data:
+                if msg.deserialize(array('B',data)):
+                    print "\tRX: len(%d) %s" % (len(data), msg.getContent())
+                    return msg
+                else:
+                    print "msg: %s" % array('B',data)
+                    raise BluetoothError ("Message deserialization failed.")
+            else:
+                raise BluetoothError ("Timeout. No data received.")
+
+    def connect(self):
+        self.__connectRFCOMM()
+
+    def disconnect(self):
+        self.__disconnectRFCOMM()
+
+    def isConnected(self):
+        return self.state
+
+    def proc_connect(self):
+        try:
+            self.__sendMsg(SAPMessage_CONNECT_REQ(self.bufsize))
+            params = self.__rcvMsg(SAPMessage_CONNECT_RESP()).getParams()
+
+            if params[0].getValue() in (0x00,  0x04):
+                pass
+            elif params[0].getValue() == 0x02:
+                self.bufsize = params[1].getValue()
+
+                self.__sendMsg(SAPMessage_CONNECT_REQ(self.bufsize))
+                params = self.__rcvMsg(SAPMessage_CONNECT_RESP()).getParams()
+
+                if params[0].getValue() not in (0x00,  0x04):
+                    return False
+            else:
+                return False
+
+            params = self.__rcvMsg(SAPMessage_STATUS_IND()).getParams()
+            if params[0].getValue() == 0x00:
+                return False
+            elif params[0].getValue() == 0x01:
+                """OK, Card reset"""
+                return self.proc_transferATR()
+            elif params[0].getValue() == 0x02:
+                """T0 not supported"""
+                if self.proc_transferATR():
+                    return self.proc_setTransportProtocol(1)
+                else:
+                    return False
+            else:
+                return False
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_disconnectByClient(self, timeout=0):
+        try:
+            self.__sendMsg(SAPMessage_DISCONNECT_REQ())
+            self.__rcvMsg(SAPMessage_DISCONNECT_RESP())
+            time.sleep(timeout) # let srv to close rfcomm
+            self.__disconnectRFCOMM()
+            return True
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_disconnectByServer(self, timeout=0):
+        try:
+            params = self.__rcvMsg(SAPMessage_DISCONNECT_IND()).getParams()
+
+            """gracefull"""
+            if params[0].getValue() == 0x00:
+                if not self.proc_transferAPDU():
+                    return False
+
+            return self.proc_disconnectByClient(timeout)
+
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_transferAPDU(self,  apdu = "Sample APDU command"):
+        try:
+            self.__sendMsg(SAPMessage_TRANSFER_APDU_REQ(apdu))
+            params = self.__rcvMsg(SAPMessage_TRANSFER_APDU_RESP()).getParams()
+            return True
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_transferATR(self):
+        try:
+            self.__sendMsg(SAPMessage_TRANSFER_ATR_REQ())
+            params = self.__rcvMsg(SAPMessage_TRANSFER_ATR_RESP()).getParams()
+            return True
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_powerSimOff(self):
+        try:
+            self.__sendMsg(SAPMessage_POWER_SIM_OFF_REQ())
+            params = self.__rcvMsg(SAPMessage_POWER_SIM_OFF_RESP()).getParams()
+            return True
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_powerSimOn(self):
+        try:
+            self.__sendMsg(SAPMessage_POWER_SIM_ON_REQ())
+            params = self.__rcvMsg(SAPMessage_POWER_SIM_ON_RESP()).getParams()
+            if params[0].getValue() == 0x00:
+                return self.proc_transferATR()
+
+            return True
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_resetSim(self):
+        try:
+            self.__sendMsg(SAPMessage_RESET_SIM_REQ())
+            params = self.__rcvMsg(SAPMessage_RESET_SIM_RESP()).getParams()
+            if params[0].getValue() == 0x00:
+                return self.proc_transferATR()
+
+            return True
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_reportStatus(self):
+        try:
+            params = self.__rcvMsg(SAPMessage_STATUS_IND()).getParams()
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_transferCardReaderStatus(self):
+        try:
+            self.__sendMsg(SAPMessage_TRANSFER_CARD_READER_STATUS_REQ())
+            params = self.__rcvMsg(SAPMessage_TRANSFER_CARD_READER_STATUS_RESP()).getParams()
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_errorResponse(self):
+        try:
+            """ send malformed message, no mandatory maxmsgsize parameter"""
+            self.__sendMsg(SAPMessage_CONNECT_REQ())
+
+            params = self.__rcvMsg(SAPMessage_ERROR_RESP()).getParams()
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+    def proc_setTransportProtocol(self,  protocol = 0):
+        try:
+            self.__sendMsg(SAPMessage_SET_TRANSPORT_PROTOCOL_REQ(protocol))
+            params = self.__rcvMsg(SAPMessage_SET_TRANSPORT_PROTOCOL_RESP()).getParams()
+
+            if params[0].getValue() == 0x00:
+                params = self.__rcvMsg(SAPMessage_STATUS_IND()).getParams()
+                if params[0].getValue() in (0x01,  0x02):
+                    return self.proc_transferATR()
+                else:
+                    return True
+                    """return False ???"""
+            elif params[0].getValue == 0x07:
+                """not supported"""
+                return True
+                """return False ???"""
+            else:
+                return False
+
+        except BluetoothError , e:
+            print "Error. " +str(e)
+            return False
+
+if __name__ == "__main__":
+    pass
diff --git a/test/scotest.c b/test/scotest.c
new file mode 100644
index 0000000..50b622a
--- /dev/null
+++ b/test/scotest.c
@@ -0,0 +1,434 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <ctype.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <syslog.h>
+#include <signal.h>
+#include <sys/time.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/sco.h>
+
+/* Test modes */
+enum {
+	SEND,
+	RECV,
+	RECONNECT,
+	MULTY,
+	DUMP,
+	CONNECT
+};
+
+static unsigned char *buf;
+
+/* Default data size */
+static long data_size = 672;
+
+static bdaddr_t bdaddr;
+
+static float tv2fl(struct timeval tv)
+{
+	return (float)tv.tv_sec + (float)(tv.tv_usec/1000000.0);
+}
+
+static int do_connect(char *svr)
+{
+	struct sockaddr_sco addr;
+	struct sco_conninfo conn;
+	socklen_t optlen;
+	int sk;
+
+	/* Create socket */
+	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
+	if (sk < 0) {
+		syslog(LOG_ERR, "Can't create socket: %s (%d)",
+							strerror(errno), errno);
+		return -1;
+	}
+
+	/* Bind to local address */
+	memset(&addr, 0, sizeof(addr));
+	addr.sco_family = AF_BLUETOOTH;
+	bacpy(&addr.sco_bdaddr, &bdaddr);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Connect to remote device */
+	memset(&addr, 0, sizeof(addr));
+	addr.sco_family = AF_BLUETOOTH;
+	str2ba(svr, &addr.sco_bdaddr);
+
+	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		syslog(LOG_ERR, "Can't connect: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Get connection information */
+	memset(&conn, 0, sizeof(conn));
+	optlen = sizeof(conn);
+
+	if (getsockopt(sk, SOL_SCO, SCO_CONNINFO, &conn, &optlen) < 0) {
+		syslog(LOG_ERR, "Can't get SCO connection information: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x]",
+		conn.hci_handle,
+		conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
+
+	return sk;
+
+error:
+	close(sk);
+	return -1;
+}
+
+static void do_listen(void (*handler)(int sk))
+{
+	struct sockaddr_sco addr;
+	struct sco_conninfo conn;
+	socklen_t optlen;
+	int sk, nsk;
+	char ba[18];
+
+	/* Create socket */
+	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
+	if (sk < 0) {
+		syslog(LOG_ERR, "Can't create socket: %s (%d)",
+							strerror(errno), errno);
+		exit(1);
+	}
+
+	/* Bind to local address */
+	memset(&addr, 0, sizeof(addr));
+	addr.sco_family = AF_BLUETOOTH;
+	bacpy(&addr.sco_bdaddr, &bdaddr);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	/* Listen for connections */
+	if (listen(sk, 10)) {
+		syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
+							strerror(errno), errno);
+		goto error;
+	}
+
+	syslog(LOG_INFO,"Waiting for connection ...");
+
+	while (1) {
+		memset(&addr, 0, sizeof(addr));
+		optlen = sizeof(addr);
+
+		nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
+		if (nsk < 0) {
+			syslog(LOG_ERR,"Accept failed: %s (%d)",
+							strerror(errno), errno);
+			goto error;
+		}
+		if (fork()) {
+			/* Parent */
+			close(nsk);
+			continue;
+		}
+		/* Child */
+		close(sk);
+
+		/* Get connection information */
+		memset(&conn, 0, sizeof(conn));
+		optlen = sizeof(conn);
+
+		if (getsockopt(nsk, SOL_SCO, SCO_CONNINFO, &conn, &optlen) < 0) {
+			syslog(LOG_ERR, "Can't get SCO connection information: %s (%d)",
+							strerror(errno), errno);
+			close(nsk);
+			goto error;
+		}
+
+		ba2str(&addr.sco_bdaddr, ba);
+		syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]",
+			ba, conn.hci_handle,
+			conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
+
+		handler(nsk);
+
+		syslog(LOG_INFO, "Disconnect");
+		exit(0);
+	}
+
+	return;
+
+error:
+	close(sk);
+	exit(1);
+}
+
+static void dump_mode(int sk)
+{
+	int len;
+
+	syslog(LOG_INFO,"Receiving ...");
+	while ((len = read(sk, buf, data_size)) > 0)
+		syslog(LOG_INFO, "Recevied %d bytes", len);
+}
+
+static void recv_mode(int sk)
+{
+	struct timeval tv_beg,tv_end,tv_diff;
+	long total;
+	uint32_t seq;
+
+	syslog(LOG_INFO, "Receiving ...");
+
+	seq = 0;
+	while (1) {
+		gettimeofday(&tv_beg, NULL);
+		total = 0;
+		while (total < data_size) {
+			int r;
+			if ((r = recv(sk, buf, data_size, 0)) <= 0) {
+				if (r < 0)
+					syslog(LOG_ERR, "Read failed: %s (%d)",
+							strerror(errno), errno);
+				return;
+			}
+			total += r;
+		}
+		gettimeofday(&tv_end, NULL);
+
+		timersub(&tv_end, &tv_beg, &tv_diff);
+
+		syslog(LOG_INFO,"%ld bytes in %.2fm speed %.2f kb", total,
+			tv2fl(tv_diff) / 60.0,
+			(float)( total / tv2fl(tv_diff) ) / 1024.0 );
+	}
+}
+
+static void send_mode(char *svr)
+{
+	struct sco_options so;
+	socklen_t len;
+	uint32_t seq;
+	int i, sk;
+
+	if ((sk = do_connect(svr)) < 0) {
+		syslog(LOG_ERR, "Can't connect to the server: %s (%d)",
+							strerror(errno), errno);
+		exit(1);
+	}
+
+	len = sizeof(so);
+	if (getsockopt(sk, SOL_SCO, SCO_OPTIONS, &so, &len) < 0) {
+		syslog(LOG_ERR, "Can't get SCO options: %s (%d)",
+							strerror(errno), errno);
+		exit(1);
+	}
+
+	syslog(LOG_INFO,"Sending ...");
+
+	for (i = 6; i < so.mtu; i++)
+		buf[i] = 0x7f;
+
+	seq = 0;
+	while (1) {
+		*(uint32_t *) buf = htobl(seq);
+		*(uint16_t *) (buf + 4) = htobs(data_size);
+		seq++;
+
+		if (send(sk, buf, so.mtu, 0) <= 0) {
+			syslog(LOG_ERR, "Send failed: %s (%d)",
+							strerror(errno), errno);
+			exit(1);
+		}
+
+		usleep(1);
+	}
+}
+
+static void reconnect_mode(char *svr)
+{
+	while (1) {
+		int sk;
+
+		if ((sk = do_connect(svr)) < 0) {
+			syslog(LOG_ERR, "Can't connect to the server: %s (%d)",
+							strerror(errno), errno);
+			exit(1);
+		}
+
+		close(sk);
+
+		sleep(5);
+	}
+}
+
+static void multy_connect_mode(char *svr)
+{
+	while (1) {
+		int i, sk;
+
+		for (i = 0; i < 10; i++){
+			if (fork())
+				continue;
+
+			/* Child */
+			sk = do_connect(svr);
+			if (sk < 0) {
+				syslog(LOG_ERR, "Can't connect to the server: %s (%d)",
+							strerror(errno), errno);
+			}
+			close(sk);
+			exit(0);
+		}
+
+		sleep(19);
+	}
+}
+
+static void usage(void)
+{
+	printf("scotest - SCO testing\n"
+		"Usage:\n");
+	printf("\tscotest <mode> [-b bytes] [bd_addr]\n");
+	printf("Modes:\n"
+		"\t-d dump (server)\n"
+		"\t-c reconnect (client)\n"
+		"\t-m multiple connects (client)\n"
+		"\t-r receive (server)\n"
+		"\t-s connect and send (client)\n"
+		"\t-n connect and be silent (client)\n");
+}
+
+int main(int argc ,char *argv[])
+{
+	struct sigaction sa;
+	int opt, sk, mode = RECV;
+
+	while ((opt=getopt(argc,argv,"rdscmnb:")) != EOF) {
+		switch(opt) {
+		case 'r':
+			mode = RECV;
+			break;
+
+		case 's':
+			mode = SEND;
+			break;
+
+		case 'd':
+			mode = DUMP;
+			break;
+
+		case 'c':
+			mode = RECONNECT;
+			break;
+
+		case 'm':
+			mode = MULTY;
+			break;
+
+		case 'n':
+			mode = CONNECT;
+			break;
+
+		case 'b':
+			data_size = atoi(optarg);
+			break;
+
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	if (!(argc - optind) && (mode != RECV && mode != DUMP)) {
+		usage();
+		exit(1);
+	}
+
+	if (!(buf = malloc(data_size))) {
+		perror("Can't allocate data buffer");
+		exit(1);
+	}
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_handler = SIG_IGN;
+	sa.sa_flags   = SA_NOCLDSTOP;
+	sigaction(SIGCHLD, &sa, NULL);
+
+	openlog("scotest", LOG_PERROR | LOG_PID, LOG_LOCAL0);
+
+	switch( mode ){
+		case RECV:
+			do_listen(recv_mode);
+			break;
+
+		case DUMP:
+			do_listen(dump_mode);
+			break;
+
+		case SEND:
+			send_mode(argv[optind]);
+			break;
+
+		case RECONNECT:
+			reconnect_mode(argv[optind]);
+			break;
+
+		case MULTY:
+			multy_connect_mode(argv[optind]);
+			break;
+
+		case CONNECT:
+			sk = do_connect(argv[optind]);
+			if (sk < 0)
+				exit(1);
+			dump_mode(sk);
+			break;
+	}
+
+	syslog(LOG_INFO, "Exit");
+
+	closelog();
+
+	return 0;
+}
diff --git a/test/sdptest.c b/test/sdptest.c
new file mode 100644
index 0000000..480a468
--- /dev/null
+++ b/test/sdptest.c
@@ -0,0 +1,146 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2005-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <signal.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+static volatile sig_atomic_t __io_finished = 0;
+
+static void callback(uint8_t type, uint16_t status,
+				uint8_t *rsp, size_t size, void *udata)
+{
+	unsigned int i;
+
+	for (i = 0; i < size; i++) {
+		printf("%02x ", rsp[i]);
+		if ((i + 1) % 8 == 0)
+			printf(" ");
+		if ((i + 1) % 16 == 0)
+			printf("\n");
+	}
+	printf("\n");
+
+	__io_finished = 1;
+}
+
+static void cmd_search(bdaddr_t *src, bdaddr_t *dst)
+{
+	sdp_session_t *session;
+	sdp_list_t *search, *attrids;
+	uint32_t range = 0x0000ffff;
+	uuid_t uuid;
+
+	session = sdp_connect(src, dst, 0);
+	if (!session) {
+		perror("Can't connect to SDP service");
+		exit(1);
+	}
+
+	sdp_set_notify(session, callback, NULL);
+
+	sdp_uuid16_create(&uuid, PUBLIC_BROWSE_GROUP);
+
+	search = sdp_list_append(NULL, &uuid);
+
+	attrids = sdp_list_append(NULL, &range);
+
+	//sdp_service_search_attr_async(session, search,
+	//				SDP_ATTR_REQ_RANGE, attrids);
+
+	sdp_service_search_async(session, search, 0xffff);
+
+	sdp_list_free(attrids, NULL);
+
+	sdp_list_free(search, NULL);
+
+	while (!__io_finished)
+		sdp_process(session);
+
+	sdp_close(session);
+}
+
+static void usage(void)
+{
+	printf("sdptest - Utility for SDP testing\n\n");
+	printf("Usage:\n"
+		"\tsdptest [-i <dev>] <bdaddr>\n");
+}
+
+static struct option main_options[] = {
+	{ "device",	1, 0, 'i' },
+	{ "help",	0, 0, 'h' },
+	{ 0, 0, 0, 0 }
+};
+
+int main(int argc, char *argv[])
+{
+	bdaddr_t src, dst;
+	int opt;
+
+	bacpy(&src, BDADDR_ANY);
+
+	while ((opt=getopt_long(argc, argv, "+i:h", main_options, NULL)) != -1) {
+		switch (opt) {
+		case 'i':
+			if (!strncasecmp(optarg, "hci", 3))
+				hci_devba(atoi(optarg + 3), &src);
+			else
+				str2ba(optarg, &dst);
+			break;
+
+		case 'h':
+		default:
+			usage();
+			exit(0);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+	optind = 0;
+
+	if (argc < 1) {
+		usage();
+		exit(1);
+	}
+
+	str2ba(argv[0], &dst);
+
+	cmd_search(&src, &dst);
+
+	return 0;
+}
diff --git a/test/service-did.xml b/test/service-did.xml
new file mode 100644
index 0000000..52eb68c
--- /dev/null
+++ b/test/service-did.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<record>
+  <attribute id="0x0001">
+    <sequence>
+      <uuid value="0x1200"/>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0200">
+    <uint16 value="0x0102" name="id"/>
+  </attribute>
+
+  <attribute id="0x0201">
+    <uint16 value="0x0a12" name="vendor"/>
+  </attribute>
+
+  <attribute id="0x0202">
+    <uint16 value="0x4711" name="product"/>
+  </attribute>
+
+  <attribute id="0x0203">
+    <uint16 value="0x0000" name="version"/>
+  </attribute>
+
+  <attribute id="0x0204">
+    <boolean value="true"/>
+  </attribute>
+
+  <attribute id="0x0205">
+    <uint16 value="0x0002" name="source"/>
+  </attribute>
+</record>
diff --git a/test/service-ftp.xml b/test/service-ftp.xml
new file mode 100644
index 0000000..1bda885
--- /dev/null
+++ b/test/service-ftp.xml
@@ -0,0 +1,37 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<record>
+  <attribute id="0x0001">
+    <sequence>
+      <uuid value="0x1106"/>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0004">
+    <sequence>
+      <sequence>
+        <uuid value="0x0100"/>
+      </sequence>
+      <sequence>
+        <uuid value="0x0003"/>
+        <uint8 value="23" name="channel"/>
+      </sequence>
+      <sequence>
+        <uuid value="0x0008"/>
+      </sequence>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0009">
+    <sequence>
+      <sequence>
+        <uuid value="0x1106"/>
+        <uint16 value="0x0100" name="version"/>
+      </sequence>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0100">
+    <text value="OBEX File Transfer" name="name"/>
+  </attribute>
+</record>
diff --git a/test/service-opp.xml b/test/service-opp.xml
new file mode 100644
index 0000000..351b4a4
--- /dev/null
+++ b/test/service-opp.xml
@@ -0,0 +1,50 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<record>
+  <attribute id="0x0001">
+    <sequence>
+      <uuid value="0x1105"/>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0004">
+    <sequence>
+      <sequence>
+        <uuid value="0x0100"/>
+      </sequence>
+      <sequence>
+        <uuid value="0x0003"/>
+        <uint8 value="23" name="channel"/>
+      </sequence>
+      <sequence>
+        <uuid value="0x0008"/>
+      </sequence>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0009">
+    <sequence>
+      <sequence>
+        <uuid value="0x1105"/>
+        <uint16 value="0x0100" name="version"/>
+      </sequence>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0100">
+    <text value="OBEX Object Push" name="name"/>
+  </attribute>
+
+  <attribute id="0x0303">
+    <sequence>
+      <uint8 value="0x01"/>
+      <uint8 value="0x01"/>
+      <uint8 value="0x02"/>
+      <uint8 value="0x03"/>
+      <uint8 value="0x04"/>
+      <uint8 value="0x05"/>
+      <uint8 value="0x06"/>
+      <uint8 value="0xff"/>
+    </sequence>
+  </attribute>
+</record>
diff --git a/test/service-record.dtd b/test/service-record.dtd
new file mode 100644
index 0000000..f53be5d
--- /dev/null
+++ b/test/service-record.dtd
@@ -0,0 +1,66 @@
+<!ELEMENT record (attribute)*>
+
+<!ELEMENT attribute (sequence|alternate|text|url|uuid|boolean|uint8|uint16|uint32|uint64|nil)+>
+<!ATTLIST attribute id CDATA #REQUIRED>
+
+<!ELEMENT sequence (sequence|alternate|text|url|uuid|boolean|uint8|uint16|uint32|uint64|uint128|int8|int16|int32|int64|int128|nil)+>
+
+<!ELEMENT alternate (sequence|alternate|text|url|uuid|boolean|uint8|uint16|uint32|uint64|uint128|int8|int16|int32|int64|int128|nil)+>
+
+<!ELEMENT text EMPTY>
+<!ATTLIST text value CDATA #REQUIRED>
+<!ATTLIST text name CDATA>
+<!ATTLIST text encoding (normal|hex) "normal">
+
+<!ELEMENT url EMPTY>
+<!ATTLIST url value CDATA #REQUIRED>
+<!ATTLIST url name CDATA>
+
+<!ELEMENT uuid EMPTY>
+<!ATTLIST uuid value CDATA #REQUIRED>
+
+<!ELEMENT boolean EMPTY>
+<!ATTLIST boolean value CDATA #REQUIRED>
+<!ATTLIST boolean name CDATA>
+
+<!ELEMENT uint8 EMPTY>
+<!ATTLIST uint8 value CDATA #REQUIRED>
+<!ATTLIST uint8 name CDATA>
+
+<!ELEMENT uint16 EMPTY>
+<!ATTLIST uint16 value CDATA #REQUIRED>
+<!ATTLIST uint16 name CDATA>
+
+<!ELEMENT uint32 EMPTY>
+<!ATTLIST uint32 value CDATA #REQUIRED>
+<!ATTLIST uint32 name CDATA>
+
+<!ELEMENT uint64 EMPTY>
+<!ATTLIST uint64 value CDATA #REQUIRED>
+<!ATTLIST uint64 name CDATA>
+
+<!ELEMENT uint128 EMPTY>
+<!ATTLIST uint128 value CDATA #REQUIRED>
+<!ATTLIST uint128 name CDATA>
+
+<!ELEMENT int8 EMPTY>
+<!ATTLIST int8 value CDATA #REQUIRED>
+<!ATTLIST int8 name CDATA>
+
+<!ELEMENT int16 EMPTY>
+<!ATTLIST int16 value CDATA #REQUIRED>
+<!ATTLIST int16 name CDATA>
+
+<!ELEMENT int32 EMPTY>
+<!ATTLIST int32 value CDATA #REQUIRED>
+<!ATTLIST int32 name CDATA>
+
+<!ELEMENT int64 EMPTY>
+<!ATTLIST int64 value CDATA #REQUIRED>
+<!ATTLIST int64 name CDATA>
+
+<!ELEMENT int128 EMPTY>
+<!ATTLIST int128 value CDATA #REQUIRED>
+<!ATTLIST int128 name CDATA>
+
+<!ELEMENT nil EMPTY>
diff --git a/test/service-spp.xml b/test/service-spp.xml
new file mode 100644
index 0000000..2b156c3
--- /dev/null
+++ b/test/service-spp.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<record>
+  <attribute id="0x0001">
+    <sequence>
+      <uuid value="0x1101"/>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0004">
+    <sequence>
+      <sequence>
+        <uuid value="0x0100"/>
+      </sequence>
+      <sequence>
+        <uuid value="0x0003"/>
+        <uint8 value="23" name="channel"/>
+      </sequence>
+    </sequence>
+  </attribute>
+
+  <attribute id="0x0100">
+    <text value="COM5" name="name"/>
+  </attribute>
+</record>
diff --git a/test/simple-agent b/test/simple-agent
new file mode 100755
index 0000000..f2cc3dd
--- /dev/null
+++ b/test/simple-agent
@@ -0,0 +1,120 @@
+#!/usr/bin/python
+
+import gobject
+
+import sys
+import dbus
+import dbus.service
+import dbus.mainloop.glib
+
+class Rejected(dbus.DBusException):
+	_dbus_error_name = "org.bluez.Error.Rejected"
+
+class Agent(dbus.service.Object):
+	exit_on_release = True
+
+	def set_exit_on_release(self, exit_on_release):
+		self.exit_on_release = exit_on_release
+
+	@dbus.service.method("org.bluez.Agent",
+					in_signature="", out_signature="")
+	def Release(self):
+		print "Release"
+		if self.exit_on_release:
+			mainloop.quit()
+
+	@dbus.service.method("org.bluez.Agent",
+					in_signature="os", out_signature="")
+	def Authorize(self, device, uuid):
+		print "Authorize (%s, %s)" % (device, uuid)
+		authorize = raw_input("Authorize connection (yes/no): ")
+		if (authorize == "yes"):
+			return
+		raise Rejected("Connection rejected by user")
+
+	@dbus.service.method("org.bluez.Agent",
+					in_signature="o", out_signature="s")
+	def RequestPinCode(self, device):
+		print "RequestPinCode (%s)" % (device)
+		return raw_input("Enter PIN Code: ")
+
+	@dbus.service.method("org.bluez.Agent",
+					in_signature="o", out_signature="u")
+	def RequestPasskey(self, device):
+		print "RequestPasskey (%s)" % (device)
+		passkey = raw_input("Enter passkey: ")
+		return dbus.UInt32(passkey)
+
+	@dbus.service.method("org.bluez.Agent",
+					in_signature="ou", out_signature="")
+	def DisplayPasskey(self, device, passkey):
+		print "DisplayPasskey (%s, %d)" % (device, passkey)
+
+	@dbus.service.method("org.bluez.Agent",
+					in_signature="ou", out_signature="")
+	def RequestConfirmation(self, device, passkey):
+		print "RequestConfirmation (%s, %d)" % (device, passkey)
+		confirm = raw_input("Confirm passkey (yes/no): ")
+		if (confirm == "yes"):
+			return
+		raise Rejected("Passkey doesn't match")
+
+	@dbus.service.method("org.bluez.Agent",
+					in_signature="s", out_signature="")
+	def ConfirmModeChange(self, mode):
+		print "ConfirmModeChange (%s)" % (mode)
+		authorize = raw_input("Authorize mode change (yes/no): ")
+		if (authorize == "yes"):
+			return
+		raise Rejected("Mode change by user")
+
+	@dbus.service.method("org.bluez.Agent",
+					in_signature="", out_signature="")
+	def Cancel(self):
+		print "Cancel"
+
+def create_device_reply(device):
+	print "New device (%s)" % (device)
+	mainloop.quit()
+
+def create_device_error(error):
+	print "Creating device failed: %s" % (error)
+	mainloop.quit()
+
+if __name__ == '__main__':
+	dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+
+	bus = dbus.SystemBus()
+	manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+							"org.bluez.Manager")
+
+	if len(sys.argv) > 1:
+		path = manager.FindAdapter(sys.argv[1])
+	else:
+		path = manager.DefaultAdapter()
+
+	adapter = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Adapter")
+
+	path = "/test/agent"
+	agent = Agent(bus, path)
+
+	mainloop = gobject.MainLoop()
+
+	if len(sys.argv) > 2:
+		if len(sys.argv) > 3:
+			device = adapter.FindDevice(sys.argv[2])
+			adapter.RemoveDevice(device)
+
+		agent.set_exit_on_release(False)
+		adapter.CreatePairedDevice(sys.argv[2], path, "DisplayYesNo",
+					reply_handler=create_device_reply,
+					error_handler=create_device_error)
+	else:
+		adapter.RegisterAgent(path, "DisplayYesNo")
+		print "Agent registered"
+
+	mainloop.run()
+
+	#adapter.UnregisterAgent(path)
+	#print "Agent unregistered"
diff --git a/test/simple-endpoint b/test/simple-endpoint
new file mode 100755
index 0000000..e09a528
--- /dev/null
+++ b/test/simple-endpoint
@@ -0,0 +1,126 @@
+#!/usr/bin/python
+
+import sys
+import dbus
+import dbus.service
+import dbus.mainloop.glib
+import gobject
+
+A2DP_SOURCE_UUID = "0000110A-0000-1000-8000-00805F9B34FB"
+A2DP_SINK_UUID = "0000110B-0000-1000-8000-00805F9B34FB"
+HFP_AG_UUID = "0000111F-0000-1000-8000-00805F9B34FB"
+HSP_AG_UUID = "00001112-0000-1000-8000-00805F9B34FB"
+
+SBC_CODEC = dbus.Byte(0x00)
+#Channel Modes: Mono DualChannel Stereo JointStereo
+#Frequencies: 16Khz 32Khz 44.1Khz 48Khz
+#Subbands: 4 8
+#Blocks: 4 8 12 16
+#Bitpool Range: 2-64
+SBC_CAPABILITIES = dbus.Array([dbus.Byte(0xff), dbus.Byte(0xff), dbus.Byte(2), dbus.Byte(64)])
+# JointStereo 44.1Khz Subbands: Blocks: 16 Bitpool Range: 2-32
+SBC_CONFIGURATION = dbus.Array([dbus.Byte(0x21), dbus.Byte(0x15), dbus.Byte(2), dbus.Byte(32)])
+
+MP3_CODEC = dbus.Byte(0x01)
+#Channel Modes: Mono DualChannel Stereo JointStereo
+#Frequencies: 32Khz 44.1Khz 48Khz
+#CRC: YES
+#Layer: 3
+#Bit Rate: All except Free format
+#VBR: Yes
+#Payload Format: RFC-2250
+MP3_CAPABILITIES = dbus.Array([dbus.Byte(0x3f), dbus.Byte(0x07), dbus.Byte(0xff), dbus.Byte(0xfe)])
+# JointStereo 44.1Khz Layer: 3 Bit Rate: VBR Format: RFC-2250
+MP3_CONFIGURATION = dbus.Array([dbus.Byte(0x21), dbus.Byte(0x02), dbus.Byte(0x00), dbus.Byte(0x80)])
+
+PCM_CODEC = dbus.Byte(0x00)
+PCM_CONFIGURATION = dbus.Array([], signature="ay")
+
+class Rejected(dbus.DBusException):
+	_dbus_error_name = "org.bluez.Error.Rejected"
+
+class Endpoint(dbus.service.Object):
+	exit_on_release = True
+	configuration = SBC_CONFIGURATION
+
+	def set_exit_on_release(self, exit_on_release):
+		self.exit_on_release = exit_on_release
+
+	def default_configuration(self, configuration):
+		self.configuration = configuration
+
+	@dbus.service.method("org.bluez.MediaEndpoint",
+					in_signature="", out_signature="")
+	def Release(self):
+		print "Release"
+		if self.exit_on_release:
+			mainloop.quit()
+
+	@dbus.service.method("org.bluez.MediaEndpoint",
+					in_signature="", out_signature="")
+	def ClearConfiguration(self):
+		print "ClearConfiguration"
+
+	@dbus.service.method("org.bluez.MediaEndpoint",
+					in_signature="oay", out_signature="")
+	def SetConfiguration(self, transport, config):
+		print "SetConfiguration (%s, %s)" % (transport, config)
+		return
+
+	@dbus.service.method("org.bluez.MediaEndpoint",
+					in_signature="ay", out_signature="ay")
+	def SelectConfiguration(self, caps):
+		print "SelectConfiguration (%s)" % (caps)
+		return self.configuration
+
+if __name__ == '__main__':
+	dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+
+	bus = dbus.SystemBus()
+	manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+						"org.bluez.Manager")
+
+	if len(sys.argv) > 1:
+		path = manager.FindAdapter(sys.argv[1])
+	else:
+		path = manager.DefaultAdapter()
+
+	media = dbus.Interface(bus.get_object("org.bluez", path),
+						"org.bluez.Media")
+
+	path = "/test/endpoint"
+	endpoint = Endpoint(bus, path)
+	mainloop = gobject.MainLoop()
+
+	properties = dbus.Dictionary({ "UUID" : A2DP_SOURCE_UUID,
+					"Codec" : SBC_CODEC,
+					"DelayReporting" : True,
+					"Capabilities" : SBC_CAPABILITIES })
+
+	if len(sys.argv) > 2:
+		if sys.argv[2] == "sbcsink":
+			properties = dbus.Dictionary({ "UUID" : A2DP_SINK_UUID,
+							"Codec" : SBC_CODEC,
+							"DelayReporting" : True,
+							"Capabilities" : SBC_CAPABILITIES })
+		if sys.argv[2] == "mp3source":
+			properties = dbus.Dictionary({ "UUID" : A2DP_SOURCE_UUID,
+							"Codec" : MP3_CODEC,
+							"Capabilities" : MP3_CAPABILITIES })
+			endpoint.default_configuration(MP3_CONFIGURATION)
+		if sys.argv[2] == "mp3sink":
+			properties = dbus.Dictionary({ "UUID" : A2DP_SINK_UUID,
+							"Codec" : MP3_CODEC,
+							"Capabilities" : MP3_CAPABILITIES })
+			endpoint.default_configuration(MP3_CONFIGURATION)
+		if sys.argv[2] == "hfpag" or sys.argv[2] == "hspag":
+			properties = dbus.Dictionary({ "UUID" : HFP_AG_UUID,
+							"Codec" : PCM_CODEC,
+							"Capabilities" :  PCM_CONFIGURATION })
+			endpoint.default_configuration(dbus.Array([]))
+
+	print properties
+
+	media.RegisterEndpoint(path, properties)
+
+	mainloop.run()
diff --git a/test/simple-service b/test/simple-service
new file mode 100755
index 0000000..d03ec3d
--- /dev/null
+++ b/test/simple-service
@@ -0,0 +1,127 @@
+#!/usr/bin/python
+
+import sys
+import time
+import dbus
+
+xml = ' \
+<?xml version="1.0" encoding="UTF-8" ?> 	\
+<record>					\
+  <attribute id="0x0001">			\
+    <sequence>					\
+      <uuid value="0x1101"/>			\
+    </sequence>					\
+  </attribute>					\
+						\
+  <attribute id="0x0002">			\
+     <uint32 value="0"/>			\
+  </attribute>					\
+						\
+  <attribute id="0x0003">			\
+    <uuid value="00001101-0000-1000-8000-00805f9b34fb"/> \
+  </attribute>					\
+						\
+  <attribute id="0x0004">			\
+    <sequence>					\
+      <sequence>				\
+        <uuid value="0x0100"/>			\
+      </sequence>				\
+      <sequence>				\
+        <uuid value="0x0003"/>			\
+        <uint8 value="23"/>			\
+      </sequence>				\
+    </sequence>					\
+  </attribute>					\
+						\
+  <attribute id="0x0005">			\
+    <sequence>					\
+      <uuid value="0x1002"/>			\
+    </sequence>					\
+  </attribute>					\
+						\
+  <attribute id="0x0006">			\
+    <sequence>					\
+      <uint16 value="0x656e"/>			\
+      <uint16 value="0x006a"/>			\
+      <uint16 value="0x0100"/>			\
+    </sequence>					\
+  </attribute>					\
+						\
+  <attribute id="0x0007">			\
+     <uint32 value="0"/>			\
+  </attribute>					\
+						\
+  <attribute id="0x0008">			\
+     <uint8 value="0xff"/>			\
+  </attribute>					\
+						\
+  <attribute id="0x0009">			\
+    <sequence>					\
+      <sequence>				\
+        <uuid value="0x1101"/>			\
+        <uint16 value="0x0100"/>		\
+      </sequence>				\
+    </sequence>					\
+  </attribute>					\
+						\
+  <attribute id="0x000a">			\
+    <url value="http://www.bluez.org/"/>	\
+  </attribute>					\
+						\
+  <attribute id="0x000b">			\
+    <url value="http://www.bluez.org/"/>	\
+  </attribute>					\
+						\
+  <attribute id="0x000c">			\
+    <url value="http://www.bluez.org/"/>	\
+  </attribute>					\
+						\
+  <attribute id="0x0100">			\
+    <text value="Serial Port"/>			\
+  </attribute>					\
+						\
+  <attribute id="0x0101">			\
+    <text value="Serial Port Service"/>		\
+  </attribute>					\
+						\
+  <attribute id="0x0102">			\
+     <text value="BlueZ"/>			\
+  </attribute>					\
+						\
+  <attribute id="0x0200">			\
+    <sequence>					\
+      <uint16 value="0x0100"/>			\
+    </sequence>					\
+  </attribute>					\
+						\
+  <attribute id="0x0201">			\
+     <uint32 value="0"/>			\
+  </attribute>					\
+</record>					\
+'
+
+bus = dbus.SystemBus()
+manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+						"org.bluez.Manager")
+
+if len(sys.argv) > 1:
+	path = manager.FindAdapter(sys.argv[1])
+else:
+	path = manager.DefaultAdapter()
+
+service = dbus.Interface(bus.get_object("org.bluez", path),
+						"org.bluez.Service")
+
+handle = service.AddRecord(xml)
+
+print "Service record with handle 0x%04x added" % (handle)
+
+print "Press CTRL-C to remove service record"
+
+try:
+	time.sleep(1000)
+	print "Terminating session"
+except:
+	pass
+
+service.RemoveRecord(dbus.UInt32(handle))
diff --git a/test/test-adapter b/test/test-adapter
new file mode 100755
index 0000000..00ef6f5
--- /dev/null
+++ b/test/test-adapter
@@ -0,0 +1,120 @@
+#!/usr/bin/python
+
+import sys
+import dbus
+import time
+from optparse import OptionParser, make_option
+
+bus = dbus.SystemBus()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.bluez.Manager")
+
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+if (len(args) < 1):
+	print "Usage: %s <command>" % (sys.argv[0])
+	print ""
+	print "  address"
+	print "  name [name]"
+	print "  powered [on/off]"
+	print "  pairable [on/off]"
+	print "  pairabletimeout [timeout]"
+	print "  discoverable [on/off]"
+	print "  discoverabletimeout [timeout]"
+	print "  discovering"
+	sys.exit(1)
+
+if (args[0] == "address"):
+	properties = adapter.GetProperties()
+	print properties["Address"]
+	sys.exit(0)
+
+if (args[0] == "name"):
+	if (len(args) < 2):
+		properties = adapter.GetProperties()
+		print properties["Name"]
+	else:
+		adapter.SetProperty("Name", args[1])
+	sys.exit(0)
+
+if (args[0] == "powered"):
+	if (len(args) < 2):
+		properties = adapter.GetProperties()
+		print properties["Powered"]
+	else:
+		if (args[1] == "on"):
+			value = dbus.Boolean(1)
+		elif (args[1] == "off"):
+			value = dbus.Boolean(0)
+		else:
+			value = dbus.Boolean(args[1])
+		adapter.SetProperty("Powered", value)
+	sys.exit(0)
+
+if (args[0] == "pairable"):
+	if (len(args) < 2):
+		properties = adapter.GetProperties()
+		print properties["Pairable"]
+	else:
+		if (args[1] == "on"):
+			value = dbus.Boolean(1)
+		elif (args[1] == "off"):
+			value = dbus.Boolean(0)
+		else:
+			value = dbus.Boolean(args[1])
+		adapter.SetProperty("Pairable", value)
+	sys.exit(0)
+
+if (args[0] == "pairabletimeout"):
+	if (len(args) < 2):
+		properties = adapter.GetProperties()
+		print properties["PairableTimeout"]
+	else:
+		timeout = dbus.UInt32(args[1])
+		adapter.SetProperty("PairableTimeout", timeout)
+	sys.exit(0)
+
+if (args[0] == "discoverable"):
+	if (len(args) < 2):
+		properties = adapter.GetProperties()
+		print properties["Discoverable"]
+	else:
+		if (args[1] == "on"):
+			value = dbus.Boolean(1)
+		elif (args[1] == "off"):
+			value = dbus.Boolean(0)
+		else:
+			value = dbus.Boolean(args[1])
+		adapter.SetProperty("Discoverable", value)
+	sys.exit(0)
+
+if (args[0] == "discoverabletimeout"):
+	if (len(args) < 2):
+		properties = adapter.GetProperties()
+		print properties["DiscoverableTimeout"]
+	else:
+		timeout = dbus.UInt32(args[1])
+		adapter.SetProperty("DiscoverableTimeout", timeout)
+	sys.exit(0)
+
+if (args[0] == "discovering"):
+	properties = adapter.GetProperties()
+	print properties["Discovering"]
+	sys.exit(0)
+
+print "Unknown command"
+sys.exit(1)
diff --git a/test/test-attrib b/test/test-attrib
new file mode 100755
index 0000000..b9e83c5
--- /dev/null
+++ b/test/test-attrib
@@ -0,0 +1,108 @@
+#!/usr/bin/python
+# Script for testing the Attribute D-Bus API
+
+import sys
+from optparse import OptionParser, OptionValueError
+from binascii import hexlify, unhexlify
+
+import gobject
+
+import sys
+import dbus
+import dbus.mainloop.glib
+from optparse import OptionParser, make_option
+
+dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+bus = dbus.SystemBus()
+mainloop = gobject.MainLoop()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.bluez.Manager")
+
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+if (len(args) < 1):
+	print "Usage: %s <command>" % (sys.argv[0])
+	print ""
+	print "	 list"
+	print "	 services <address>"
+	print "	 discover <service path>"
+	print "	 chars <service path>"
+	sys.exit(1)
+
+if (args[0] == "list"):
+	for path in adapter.ListDevices():
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		devprop = device.GetProperties()
+		print "[ %s ]" % devprop["Address"]
+		for path in devprop["Services"]:
+
+			service = dbus.Interface(bus.get_object("org.bluez", path),
+									 "org.bluez.Characteristic")
+			srvprop = service.GetProperties()
+			print " * %s" % (path)
+			print "	UUID: %s" % srvprop["UUID"]
+			print "	Chars: ",
+			for char in srvprop["Characteristics"]:
+				print "%s " % char,
+			print
+			print
+		print
+	sys.exit(0)
+
+if (args[0] == "services"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		properties = device.GetProperties()
+		for path in properties["Services"]:
+			print path
+	sys.exit(0)
+
+if (args[0] == "discover"):
+	if (len(args) < 2):
+		print "Need service path parameter"
+	else:
+		service = dbus.Interface(bus.get_object("org.bluez", args[1]),
+							"org.bluez.Characteristic")
+		for path in service.DiscoverCharacteristics():
+			print path
+	sys.exit(0)
+
+if (args[0] == "chars"):
+	if (len(args) < 2):
+		print "Need service path parameter"
+	else:
+		service = dbus.Interface(bus.get_object("org.bluez", args[1]),
+								 "org.bluez.Characteristic")
+		srvprop = service.GetProperties()
+		for path in srvprop["Characteristics"]:
+			print "[ %s ]" % (path)
+			char = dbus.Interface(bus.get_object("org.bluez", path),
+								 "org.bluez.Characteristic")
+			charprop = char.GetProperties()
+			print "	Name: %s" % charprop["Name"]
+			print "	UUID: %s" % charprop["UUID"]
+			print
+		print
+	sys.exit(0)
+
+print "Unknown command"
+sys.exit(1)
diff --git a/test/test-audio b/test/test-audio
new file mode 100755
index 0000000..8b7a62d
--- /dev/null
+++ b/test/test-audio
@@ -0,0 +1,45 @@
+#!/usr/bin/python
+
+import sys
+import dbus
+from optparse import OptionParser, make_option
+
+bus = dbus.SystemBus()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.bluez.Manager")
+
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+if len(args) < 2:
+	print """Usage: %s <command>
+
+	connect <bdaddr>
+	disconnect <bdaddr>
+	""" % sys.argv[0]
+	sys.exit(1)
+
+device = adapter.FindDevice(args[1])
+audio = dbus.Interface(bus.get_object("org.bluez", device),
+				"org.bluez.Audio")
+
+if args[0] == "connect":
+	audio.Connect()
+elif args[0] == "disconnect":
+	audio.Disconnect()
+else:
+	print "Unknown command"
+	sys.exit(1)
diff --git a/test/test-device b/test/test-device
new file mode 100755
index 0000000..154af19
--- /dev/null
+++ b/test/test-device
@@ -0,0 +1,207 @@
+#!/usr/bin/python
+
+import gobject
+
+import sys
+import dbus
+import dbus.mainloop.glib
+import re
+from optparse import OptionParser, make_option
+
+dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+bus = dbus.SystemBus()
+mainloop = gobject.MainLoop()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.bluez.Manager")
+
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+if (len(args) < 1):
+	print "Usage: %s <command>" % (sys.argv[0])
+	print ""
+	print "  list"
+	print "  services <address>"
+	print "  create <address>"
+	print "  remove <address|path>"
+	print "  disconnect <address>"
+	print "  discover <address> [pattern]"
+	print "  class <address>"
+	print "  name <address>"
+	print "  alias <address> [alias]"
+	print "  trusted <address> [yes/no]"
+	print "  blocked <address> [yes/no]"
+	sys.exit(1)
+
+if (args[0] == "list"):
+	for path in adapter.ListDevices():
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		properties = device.GetProperties()
+		print "%s %s" % (properties["Address"], properties["Alias"])
+
+	sys.exit(0)
+
+def create_device_reply(device):
+	print "New device (%s)" % device
+	mainloop.quit()
+	sys.exit(0)
+
+def create_device_error(error):
+	print "Creating device failed: %s" % error
+	mainloop.quit()
+	sys.exit(1)
+
+if (args[0] == "create"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		adapter.CreateDevice(args[1],
+				reply_handler=create_device_reply,
+				error_handler=create_device_error)
+	mainloop.run()
+
+if (args[0] == "remove"):
+	if (len(args) < 2):
+		print "Need address or object path parameter"
+	else:
+		try:
+			path = adapter.FindDevice(args[1])
+		except:
+			path = args[1]
+		adapter.RemoveDevice(path)
+	sys.exit(0)
+
+if (args[0] == "disconnect"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		device.Disconnect()
+	sys.exit(0)
+
+if (args[0] == "discover"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		if (len(args) < 3):
+			pattern = ""
+		else:
+			pattern = args[2]
+		services = device.DiscoverServices(pattern);
+		for key in services.keys():
+			p = re.compile(">.*?<")
+			xml = p.sub("><", services[key].replace("\n", ""))
+			print "[ 0x%5x ]" % (key)
+			print xml
+			print
+	sys.exit(0)
+
+if (args[0] == "class"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		properties = device.GetProperties()
+		print "0x%06x" % (properties["Class"])
+	sys.exit(0)
+
+if (args[0] == "name"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		properties = device.GetProperties()
+		print properties["Name"]
+	sys.exit(0)
+
+if (args[0] == "alias"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		if (len(args) < 3):
+			properties = device.GetProperties()
+			print properties["Alias"]
+		else:
+			device.SetProperty("Alias", args[2])
+	sys.exit(0)
+
+if (args[0] == "trusted"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		if (len(args) < 3):
+			properties = device.GetProperties()
+			print properties["Trusted"]
+		else:
+			if (args[2] == "yes"):
+				value = dbus.Boolean(1)
+			elif (args[2] == "no"):
+				value = dbus.Boolean(0)
+			else:
+				value = dbus.Boolean(args[2])
+			device.SetProperty("Trusted", value)
+	sys.exit(0)
+
+if (args[0] == "blocked"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		if (len(args) < 3):
+			properties = device.GetProperties()
+			print properties["Blocked"]
+		else:
+			if (args[2] == "yes"):
+				value = dbus.Boolean(1)
+			elif (args[2] == "no"):
+				value = dbus.Boolean(0)
+			else:
+				value = dbus.Boolean(args[2])
+			device.SetProperty("Blocked", value)
+	sys.exit(0)
+
+if (args[0] == "services"):
+	if (len(args) < 2):
+		print "Need address parameter"
+	else:
+		path = adapter.FindDevice(args[1])
+		device = dbus.Interface(bus.get_object("org.bluez", path),
+							"org.bluez.Device")
+		properties = device.GetProperties()
+		for path in properties["Services"]:
+			print path
+	sys.exit(0)
+
+print "Unknown command"
+sys.exit(1)
diff --git a/test/test-discovery b/test/test-discovery
new file mode 100755
index 0000000..22c88c3
--- /dev/null
+++ b/test/test-discovery
@@ -0,0 +1,57 @@
+#!/usr/bin/python
+
+import gobject
+
+import dbus
+import dbus.mainloop.glib
+from optparse import OptionParser, make_option
+
+def device_found(address, properties):
+	print "[ " + address + " ]"
+
+	for key in properties.keys():
+		value = properties[key]
+		if (key == "Class"):
+			print "    %s = 0x%06x" % (key, value)
+		else:
+			print "    %s = %s" % (key, value)
+
+def property_changed(name, value):
+	if (name == "Discovering" and not value):
+		mainloop.quit()
+
+if __name__ == '__main__':
+	dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+
+	bus = dbus.SystemBus()
+	manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+							"org.bluez.Manager")
+
+	option_list = [
+			make_option("-i", "--device", action="store",
+					type="string", dest="dev_id"),
+			]
+	parser = OptionParser(option_list=option_list)
+
+	(options, args) = parser.parse_args()
+
+	if options.dev_id:
+		adapter_path = manager.FindAdapter(options.dev_id)
+	else:
+		adapter_path = manager.DefaultAdapter()
+
+	adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+	bus.add_signal_receiver(device_found,
+			dbus_interface = "org.bluez.Adapter",
+					signal_name = "DeviceFound")
+
+	bus.add_signal_receiver(property_changed,
+			dbus_interface = "org.bluez.Adapter",
+					signal_name = "PropertyChanged")
+
+	adapter.StartDiscovery()
+
+	mainloop = gobject.MainLoop()
+	mainloop.run()
diff --git a/test/test-input b/test/test-input
new file mode 100755
index 0000000..405bb59
--- /dev/null
+++ b/test/test-input
@@ -0,0 +1,45 @@
+#!/usr/bin/python
+
+import sys
+import dbus
+from optparse import OptionParser, make_option
+
+bus = dbus.SystemBus()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.bluez.Manager")
+
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+if len(args) < 2:
+	print """Usage: %s <command>
+
+	connect <bdaddr>
+	disconnect <bdaddr>
+	""" % sys.argv[0]
+	sys.exit(1)
+
+device = adapter.FindDevice(args[1])
+input = dbus.Interface(bus.get_object("org.bluez", device),
+				"org.bluez.Input")
+
+if args[0] == "connect":
+	input.Connect()
+elif args[0] == "disconnect":
+	input.Disconnect()
+else:
+	print "Unknown command"
+	sys.exit(1)
diff --git a/test/test-manager b/test/test-manager
new file mode 100755
index 0000000..c6cf560
--- /dev/null
+++ b/test/test-manager
@@ -0,0 +1,38 @@
+#!/usr/bin/python
+
+import gobject
+
+import dbus
+import dbus.mainloop.glib
+
+def adapter_added(path):
+	print "Adapter with path %s added" % (path)
+
+def adapter_removed(path):
+	print "Adapter with path %s removed" % (path)
+
+def default_changed(path):
+	print "Default adapter is now at path %s" % (path)
+
+if __name__ == "__main__":
+	dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+
+	bus = dbus.SystemBus()
+
+	manager = dbus.Interface(bus.get_object('org.bluez', '/'),
+							'org.bluez.Manager')
+
+	manager.connect_to_signal("AdapterAdded", adapter_added)
+
+	manager.connect_to_signal("AdapterRemoved", adapter_removed)
+
+	manager.connect_to_signal("DefaultAdapterChanged", default_changed)
+
+	try:
+		path = manager.DefaultAdapter()
+		default_changed(path)
+	except:
+		pass
+
+	mainloop = gobject.MainLoop()
+	mainloop.run()
diff --git a/test/test-network b/test/test-network
new file mode 100755
index 0000000..676fb30
--- /dev/null
+++ b/test/test-network
@@ -0,0 +1,57 @@
+#!/usr/bin/python
+
+import sys
+import time
+import dbus
+from optparse import OptionParser, make_option
+
+bus = dbus.SystemBus()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+						"org.bluez.Manager")
+
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+if (len(args) < 1):
+	print "Usage: %s <address> [service]" % (sys.argv[0])
+	sys.exit(1)
+
+address = args[0]
+
+if (len(args) < 2):
+	service = "panu"
+else:
+	service = args[1]
+
+device = adapter.FindDevice(address)
+
+network = dbus.Interface(bus.get_object("org.bluez", device),
+						"org.bluez.Network")
+
+iface = network.Connect(service)
+
+print "Connected %s to %s" % (device, address)
+
+print "Press CTRL-C to disconnect"
+
+try:
+	time.sleep(1000)
+	print "Terminating connection"
+except:
+	pass
+
+network.Disconnect()
diff --git a/test/test-sap-server b/test/test-sap-server
new file mode 100755
index 0000000..bea6ca9
--- /dev/null
+++ b/test/test-sap-server
@@ -0,0 +1,138 @@
+#!/usr/bin/python
+
+from sap import *
+import time
+
+def connect_disconnect_by_client(sap):
+
+    print "[Test] Connect - Disconnect by client \n"
+
+    try:
+        if not sap.isConnected():
+           sap.connect()
+
+        if sap.proc_connect():
+            if sap.proc_disconnectByClient():
+                print "OK"
+                return 0
+
+        print "NOT OK"
+        return 1
+
+    except BluetoothError , e:
+        print "Error " + str(e)
+
+
+def connect_disconnect_by_server_gracefully(sap, timeout=0):
+
+    print "[Test] Connect - Disconnect by server with timer \n"
+
+    try:
+        if not sap.isConnected():
+           sap.connect()
+
+        if sap.proc_connect():
+            if sap.proc_disconnectByServer(timeout):
+                print "OK"
+                return 0
+
+        print "NOT OK"
+        return 1
+
+    except BluetoothError , e:
+        print "Error " + str(e)
+
+
+def connect_txAPDU_disconnect_by_client(sap):
+
+    print "[Test] Connect - TX APDU - Disconnect by client \n"
+
+    try:
+        if not sap.isConnected():
+           sap.connect()
+
+        if sap.proc_connect():
+            if not sap.proc_transferAPDU():
+                print "NOT OK 1"
+                return 1
+
+            if not sap.proc_transferAPDU():
+                print "NOT OK 2"
+                return 1
+
+            if not sap.proc_transferAPDU():
+                print "NOT OK 3"
+                return 1
+
+            if not sap.proc_transferAPDU():
+                print "NOT OK 4"
+                return 1
+
+            if sap.proc_disconnectByClient():
+                print "OK"
+                return 0
+
+        print "NOT OK"
+        return 1
+
+    except BluetoothError , e:
+        print "Error " + str(e)
+
+def connect_rfcomm_only_and_wait_for_close_by_server(sap):
+
+    print "[Test] Connect rfcomm only  - Disconnect by server timeout \n"
+
+    if not sap.isConnected():
+       sap.connect()
+
+    time.sleep(40)
+    print "OK"
+
+def power_sim_off_on(sap):
+
+    print "[Test] Powe sim off \n"
+
+    try:
+        if not sap.isConnected():
+           sap.connect()
+
+        if sap.proc_connect():
+            if not sap.proc_resetSim():
+                print "NOT OK"
+                return 1
+
+            if not sap.proc_powerSimOff():
+                print "NOT OK"
+                return 1
+
+            if not sap.proc_powerSimOn():
+                print "NOT OK"
+                return 1
+
+            if sap.proc_disconnectByClient():
+                print "OK"
+                return 0
+
+        print "NOT OK"
+        return 1
+
+    except BluetoothError , e:
+        print "Error " + str(e)
+
+
+if __name__ == "__main__":
+
+    host = "00:00:00:00:00:0"  # server bd_addr
+    port = 8  # sap server port
+
+    try:
+        s = SAPClient(host, port)
+    except BluetoothError , e:
+        print "Error " + str(e)
+
+    connect_disconnect_by_client(s)
+    connect_disconnect_by_server_gracefully(s)
+    connect_disconnect_by_server_gracefully(s, 40)  #  wait 40 sec for srv to close rfcomm sock
+    connect_rfcomm_only_and_wait_for_close_by_server(s)
+    connect_txAPDU_disconnect_by_client(s)
+    power_sim_off_on(s)
diff --git a/test/test-serial b/test/test-serial
new file mode 100755
index 0000000..cc496df
--- /dev/null
+++ b/test/test-serial
@@ -0,0 +1,56 @@
+#!/usr/bin/python
+
+import sys
+import time
+import dbus
+from optparse import OptionParser, make_option
+
+bus = dbus.SystemBus()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"),
+						"org.bluez.Manager")
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+if (len(args) < 1):
+	print "Usage: %s <address> [service]" % (sys.argv[0])
+	sys.exit(1)
+
+address = args[0]
+
+if (len(args) < 2):
+	service = "spp"
+else:
+	service = args[1]
+
+path = adapter.FindDevice(address)
+
+serial = dbus.Interface(bus.get_object("org.bluez", path),
+						"org.bluez.Serial")
+
+node = serial.Connect(service)
+
+print "Connected %s to %s" % (node, address)
+
+print "Press CTRL-C to disconnect"
+
+try:
+	time.sleep(1000)
+	print "Terminating connection"
+except:
+	pass
+
+serial.Disconnect(node)
diff --git a/test/test-service b/test/test-service
new file mode 100755
index 0000000..8958201
--- /dev/null
+++ b/test/test-service
@@ -0,0 +1,47 @@
+#!/usr/bin/python
+
+import sys
+import dbus
+import time
+from optparse import OptionParser, make_option
+
+bus = dbus.SystemBus()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.bluez.Manager")
+
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+service = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+						"org.bluez.Service")
+
+if (len(args) < 1):
+	print "Usage: %s <command>" % (sys.argv[0])
+	print ""
+	print "  addrecord <file>"
+	sys.exit(1)
+
+if (args[0] == "addrecord"):
+	if (len(args) < 2):
+		print "Need file parameter"
+	else:
+		f = open(args[1])
+		record = f.read()
+		f.close()
+		handle = service.AddRecord(record)
+		print "0x%x" % (handle)
+		time.sleep(120)
+	sys.exit(0)
+
+print "Unknown command"
+sys.exit(1)
diff --git a/test/test-telephony b/test/test-telephony
new file mode 100755
index 0000000..5ef0ac8
--- /dev/null
+++ b/test/test-telephony
@@ -0,0 +1,176 @@
+#!/usr/bin/python
+
+import sys
+import dbus
+from optparse import OptionParser, make_option
+
+bus = dbus.SystemBus()
+
+manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.bluez.Manager")
+
+option_list = [
+		make_option("-i", "--device", action="store",
+				type="string", dest="dev_id"),
+		]
+parser = OptionParser(option_list=option_list)
+
+(options, args) = parser.parse_args()
+
+if options.dev_id:
+	adapter_path = manager.FindAdapter(options.dev_id)
+else:
+	adapter_path = manager.DefaultAdapter()
+
+adapter = dbus.Interface(bus.get_object("org.bluez", adapter_path),
+							"org.bluez.Adapter")
+
+test = dbus.Interface(bus.get_object("org.bluez", "/org/bluez/test"),
+			"org.bluez.TelephonyTest")
+
+if len(args) < 1:
+	print """Usage: %s <command>
+
+	connect <bdaddr>
+	disconnect <bdaddr>
+	outgoing <number>
+	incoming <number>
+	cancel
+	signal <level>
+	battery <level>
+	roaming <yes|no>
+	registration <status>
+	subscriber <number>
+	speakergain <bdaddr> [level]
+	microphonegain <bdaddr> [level]
+	play <bdaddr>
+	stop <bdaddr>
+	""" % sys.argv[0]
+	sys.exit(1)
+
+if args[0] == "connect":
+	if len(args) < 2:
+		print "Need device address parameter"
+		sys.exit(1)
+	device = adapter.FindDevice(args[1])
+	headset = dbus.Interface(bus.get_object("org.bluez", device),
+					"org.bluez.Headset")
+	headset.Connect()
+	sys.exit(0)
+
+if args[0] == "disconnect":
+	if len(args) < 2:
+		print "Need device address parameter"
+		sys.exit(1)
+	device = adapter.FindDevice(args[1])
+	headset = dbus.Interface(bus.get_object("org.bluez", device),
+					"org.bluez.Headset")
+	headset.Disconnect()
+	sys.exit(0)
+
+if args[0] == "speakergain":
+	if len(args) < 2:
+		print "Need device address parameter"
+		sys.exit(1)
+	device = adapter.FindDevice(args[1])
+	headset = dbus.Interface(bus.get_object("org.bluez", device),
+					"org.bluez.Headset")
+	if len(args) > 2:
+		headset.SetProperty('SpeakerGain', dbus.UInt16(args[2]))
+	else:
+		props = headset.GetProperties()
+		print props['SpeakerGain']
+
+	sys.exit(0)
+
+if args[0] == "microphonegain":
+	if len(args) < 2:
+		print "Need device address parameter"
+		sys.exit(1)
+	device = adapter.FindDevice(args[1])
+	headset = dbus.Interface(bus.get_object("org.bluez", device),
+					"org.bluez.Headset")
+	if len(args) > 2:
+		headset.SetProperty('MicrophoneGain', dbus.UInt16(args[2]))
+	else:
+		props = headset.GetProperties()
+		print props['MicrophoneGain']
+
+	sys.exit(0)
+
+if args[0] == "play":
+	if len(args) < 2:
+		print "Need device address parameter"
+		sys.exit(1)
+	device = adapter.FindDevice(args[1])
+	headset = dbus.Interface(bus.get_object("org.bluez", device),
+					"org.bluez.Headset")
+	headset.Play()
+
+	sys.exit(0)
+
+if args[0] == "stop":
+	if len(args) < 2:
+		print "Need device address parameter"
+		sys.exit(1)
+	device = adapter.FindDevice(args[1])
+	headset = dbus.Interface(bus.get_object("org.bluez", device),
+					"org.bluez.Headset")
+	headset.Stop()
+
+	sys.exit(0)
+
+if args[0] == "outgoing":
+	if len(args) > 1:
+		test.OutgoingCall(args[1])
+	else:
+		print "Need number parameter"
+	sys.exit(0)
+
+if args[0] == "incoming":
+	if len(args) > 1:
+		test.IncomingCall(args[1])
+	else:
+		print "Need number parameter"
+	sys.exit(0)
+
+if args[0] == "cancel":
+	test.CancelCall()
+	sys.exit(0)
+
+if args[0] == "signal":
+	if len(args) > 1:
+		test.SignalStrength(args[1])
+	else:
+		print "Need signal strength parameter"
+	sys.exit(0)
+
+if args[0] == "battery":
+	if len(args) > 1:
+		test.BatteryLevel(args[1])
+	else:
+		print "Need battery level parameter"
+	sys.exit(0)
+
+if args[0] == "roaming":
+	if len(args) > 1:
+		test.RoamingStatus(args[1] == "yes" or False)
+	else:
+		print "Need yes/no parameter"
+	sys.exit(0)
+
+if args[0] == "registration":
+	if len(args) > 1:
+		test.RegistrationStatus(args[1] == "yes" or False)
+	else:
+		print "Need yes/no parameter"
+	sys.exit(0)
+
+if args[0] == "subscriber":
+	if len(args) > 1:
+		test.SetSubscriberNumber(args[1])
+	else:
+		print "Need number parameter"
+	sys.exit(0)
+
+print "Unknown command"
+sys.exit(1)
diff --git a/test/test-textfile.c b/test/test-textfile.c
new file mode 100644
index 0000000..970e9e7
--- /dev/null
+++ b/test/test-textfile.c
@@ -0,0 +1,188 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "textfile.h"
+
+static void print_entry(char *key, char *value, void *data)
+{
+	printf("%s %s\n", key, value);
+}
+
+int main(int argc, char *argv[])
+{
+	char filename[] = "/tmp/textfile";
+	char key[18], value[512], *str;
+	unsigned int i, j, size, max = 10;
+	int fd, err;
+
+	size = getpagesize();
+	printf("System uses a page size of %d bytes\n\n", size);
+
+	fd = creat(filename, 0644);
+	err = ftruncate(fd, 0);
+
+	memset(value, 0, sizeof(value));
+	for (i = 0; i < (size / sizeof(value)); i++)
+		err = write(fd, value, sizeof(value));
+
+	close(fd);
+
+	sprintf(key, "11:11:11:11:11:11");
+	str = textfile_get(filename, key);
+
+	err = truncate(filename, 0);
+
+
+	sprintf(key, "00:00:00:00:00:00");
+	if (textfile_del(filename, key) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	memset(value, 0, sizeof(value));
+	if (textfile_put(filename, key, value) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	str = textfile_get(filename, key);
+	if (!str)
+		fprintf(stderr, "No value for %s\n", key);
+	else
+		free(str);
+
+	snprintf(value, sizeof(value), "Test");
+	if (textfile_put(filename, key, value) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	if (textfile_put(filename, key, value) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	if (textfile_put(filename, key, value) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	if (textfile_del(filename, key) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	str = textfile_get(filename, key);
+	if (str) {
+		fprintf(stderr, "Found value for %s\n", key);
+		free(str);
+	}
+
+	for (i = 1; i < max + 1; i++) {
+		sprintf(key, "00:00:00:00:00:%02X", i);
+
+		memset(value, 0, sizeof(value));
+		for (j = 0; j < i; j++)
+			value[j] = 'x';
+
+		printf("%s %s\n", key, value);
+
+		if (textfile_put(filename, key, value) < 0) {
+			fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+			break;
+		}
+
+		str = textfile_get(filename, key);
+		if (!str)
+			fprintf(stderr, "No value for %s\n", key);
+		else
+			free(str);
+	}
+
+
+	sprintf(key, "00:00:00:00:00:%02X", max);
+
+	memset(value, 0, sizeof(value));
+	for (j = 0; j < max; j++)
+		value[j] = 'y';
+
+	if (textfile_put(filename, key, value) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	sprintf(key, "00:00:00:00:00:%02X", 1);
+
+	memset(value, 0, sizeof(value));
+	for (j = 0; j < max; j++)
+		value[j] = 'z';
+
+	if (textfile_put(filename, key, value) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	printf("\n");
+
+	for (i = 1; i < max + 1; i++) {
+		sprintf(key, "00:00:00:00:00:%02X", i);
+
+		str = textfile_get(filename, key);
+		if (str) {
+			printf("%s %s\n", key, str);
+			free(str);
+		}
+	}
+
+
+	sprintf(key, "00:00:00:00:00:%02X", 2);
+
+	if (textfile_del(filename, key) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	sprintf(key, "00:00:00:00:00:%02X", max - 3);
+
+	if (textfile_del(filename, key) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	printf("\n");
+
+	textfile_foreach(filename, print_entry, NULL);
+
+
+	sprintf(key, "00:00:00:00:00:%02X", 1);
+
+	if (textfile_del(filename, key) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	sprintf(key, "00:00:00:00:00:%02X", max);
+
+	if (textfile_del(filename, key) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	sprintf(key, "00:00:00:00:00:%02X", max + 1);
+
+	if (textfile_del(filename, key) < 0)
+		fprintf(stderr, "%s (%d)\n", strerror(errno), errno);
+
+	printf("\n");
+
+	textfile_foreach(filename, print_entry, NULL);
+
+	return 0;
+}
diff --git a/test/uuidtest.c b/test/uuidtest.c
new file mode 100644
index 0000000..a8b46d7
--- /dev/null
+++ b/test/uuidtest.c
@@ -0,0 +1,319 @@
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/uuid.h>
+
+const char *base = "00000000-0000-1000-8000-00805F9B34FB";
+
+uint8_t xbase[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
+			0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb};
+
+uint16_t sixteen = 0x1234;
+const char *uuidsixteen128 = "00001234-0000-1000-8000-00805F9B34FB";
+const char *uuidsixteen16 = "0x1234";
+const char *uuidsixteen16a = "1234";
+
+uint8_t xuuidsixteen[] = {0x00, 0x00, 0x12, 0x34, 0x00, 0x00, 0x10, 0x00,
+			0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb};
+
+uint32_t thirtytwo = 0x12345678;
+const char *uuidthirtytwo32 = "0x12345678";
+const char *uuidthirtytwo32a = "12345678";
+const char *uuidthirtytwo128 = "12345678-0000-1000-8000-00805F9B34FB";
+
+uint8_t xuuidthirtytwo[] = {0x12, 0x34, 0x56, 0x78, 0x00, 0x00, 0x10, 0x00,
+			0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb};
+
+const char *malformed[] = {
+	"0",
+	"01",
+	"012",
+	"xxxx",
+	"xxxxx",
+	"0xxxxx",
+	"0123456",
+	"012g4567",
+	"012345678",
+	"0x234567u9",
+	"01234567890",
+	"00001234-0000-1000-8000-00805F9B34F",
+	"00001234-0000-1000-8000 00805F9B34FB",
+	"00001234-0000-1000-8000-00805F9B34FBC",
+	"00001234-0000-1000-800G-00805F9B34FB",
+	NULL,
+	};
+
+int main(int argc, char *argv[])
+{
+	bt_uuid_t u, u2, u3, u4, u5, ub, u128;
+	uint128_t n, i;
+	char buf[512];
+	int s;
+
+	memcpy(&n, xbase, 16);
+	ntoh128(&n, &i);
+
+	if (bt_string_to_uuid(&u, base)) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_string_to_uuid(&ub, base)) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u.type != 128) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (ub.type != 128) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (memcmp(&u.value.u128, &i, 16) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (memcmp(&ub.value.u128, &i, 16) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (memcmp(&ub.value.u128, &u.value.u128, 16) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u, &ub) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	bt_uuid_to_string(&u, buf, sizeof(buf));
+	/* printf("%s\n", buf); */
+
+	if (strcasecmp(buf, base) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	memcpy(&n, xuuidsixteen, 16);
+	ntoh128(&n, &i);
+
+	bt_uuid16_create(&u, sixteen);
+	bt_uuid_to_uuid128(&u, &u128);
+
+	if (bt_string_to_uuid(&u2, uuidsixteen16)) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_string_to_uuid(&u3, uuidsixteen16a)) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_string_to_uuid(&u4, uuidsixteen128)) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	bt_uuid128_create(&u5, i);
+
+	if (u.type != 16) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u128.type != 128) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u.value.u16 != sixteen) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u2.type != 16) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u3.type != 16) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u4.type != 128) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u5.type != 128) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u, &u2) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u2, &u3) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u, &u3) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u3, &u4) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u4, &u5) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u5, &u) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u, &ub) == 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u5, &ub) == 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u, &u128) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&ub, &u128) == 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	memcpy(&n, xuuidthirtytwo, 16);
+	ntoh128(&n, &i);
+
+	bt_uuid32_create(&u, thirtytwo);
+	bt_uuid_to_uuid128(&u, &u128);
+	bt_string_to_uuid(&u2, uuidthirtytwo32);
+	bt_string_to_uuid(&u3, uuidthirtytwo32a);
+	bt_string_to_uuid(&u4, uuidthirtytwo128);
+	bt_uuid128_create(&u5, i);
+
+	/*
+	bt_uuid_to_string(&u2, buf, sizeof(buf));
+	printf("%s\n", buf);
+
+	bt_uuid_to_string(&u3, buf, sizeof(buf));
+	printf("%s\n", buf);
+
+	bt_uuid_to_string(&u4, buf, sizeof(buf));
+	printf("%s\n", buf);
+	*/
+
+	if (u.type != 32) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u128.type != 128) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u.value.u32 != thirtytwo) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u2.type != 32) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u3.type != 32) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u4.type != 128) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (u5.type != 128) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u, &u2) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u2, &u3) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u3, &u4) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u4, &u5) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u5, &u) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u, &ub) == 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u5, &ub) == 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&u, &u128) != 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	if (bt_uuid_cmp(&ub, &u128) == 0) {
+		printf("Fail %d\n", __LINE__);
+		return 1;
+	}
+
+	for (s = 0; malformed[s]; ++s) {
+		if (bt_string_to_uuid(&u3, malformed[s]) == 0) {
+			printf("Fail %s %d\n", malformed[s], __LINE__);
+			return 1;
+		}
+	}
+
+	return 0;
+}