initial import, no crypto

41 files changed, 4195 insertions, 0 deletions

diff --git a/capabilityagent.cc b/capabilityagent.cc
new file mode 100644
index 0000000..3831e5d
--- /dev/null
+++ b/capabilityagent.cc
@@ -0,0 +1,66 @@
+#include "sapchannelinfo.h"
+#include "sapmanager.h"
+#include "sappeer.h"
+#include "sapsocket.h"
+#include "sapserviceinfo.h"
+#include "sapconnectionrequest.h"
+#include "capabilitypeer.h"
+#include "capabilityagent.h"
+
+static CapabilityAgent *agent = 0;
+
+CapabilityAgent::CapabilityAgent(QObject *parent)
+    : QObject(parent)
+{
+}
+
+CapabilityAgent *CapabilityAgent::instance()
+{
+	if (!agent) {
+		agent = new CapabilityAgent;
+	}
+
+	return agent;
+}
+
+void CapabilityAgent::registerServices(SAPManager *manager)
+{
+	CapabilityAgent *agent = instance();
+	SAPServiceInfo service;
+	SAPChannelInfo channel;
+
+	service.setProfile(SAProtocol::capabilityDiscoveryProfile);
+	service.setFriendlyName("CapabilityAgentProvider");
+	service.setRole(SAPServiceInfo::RoleProvider);
+	service.setVersion(1);
+	service.setConnectionTimeout(0);
+
+	channel.setChannelId(SAProtocol::capabilityDiscoveryChannel);
+	channel.setPayloadType(SAPChannelInfo::PayloadBinary);
+	channel.setQoSType(SAPChannelInfo::QoSRestricted);
+	channel.setQoSDataRate(SAPChannelInfo::QoSDataRateHigh);
+	channel.setQoSPriority(SAPChannelInfo::QoSPriorityHigh);
+	service.addChannel(channel);
+
+	manager->registerServiceAgent(service, agent);
+
+	service.setFriendlyName("CapabilityAgentConsumer");
+	service.setRole(SAPServiceInfo::RoleConsumer);
+
+	manager->registerServiceAgent(service, agent);
+}
+
+void CapabilityAgent::peerFound(SAPPeer *peer)
+{
+	new CapabilityPeer(peer, peer);
+	// We make the capability peer a child of the peer object itself,
+	// so that the peer can find it.
+}
+
+void CapabilityAgent::requestConnection(SAPConnectionRequest *request)
+{
+	SAPPeer *peer = request->peer();
+	CapabilityPeer *capPeer = peer->findChild<CapabilityPeer*>();
+
+	capPeer->requestConnection(request);
+}
diff --git a/capabilityagent.h b/capabilityagent.h
new file mode 100644
index 0000000..52692e5
--- /dev/null
+++ b/capabilityagent.h
@@ -0,0 +1,26 @@
+#ifndef CAPABILITYAGENT_H
+#define CAPABILITYAGENT_H
+
+#include <QtCore/QObject>
+#include <QtCore/QPointer>
+#include <QtCore/QHash>
+
+#include "sapagent.h"
+
+class SAPManager;
+
+class CapabilityAgent : public QObject, public SAPAgent
+{
+	Q_OBJECT
+
+	explicit CapabilityAgent(QObject *object = 0);
+
+public:
+	static CapabilityAgent* instance();
+	static void registerServices(SAPManager *manager);
+
+	void peerFound(SAPPeer *peer);
+	void requestConnection(SAPConnectionRequest *request);
+};
+
+#endif // CAPABILITYAGENT_H
diff --git a/capabilitypeer.cc b/capabilitypeer.cc
new file mode 100644
index 0000000..0290b69
--- /dev/null
+++ b/capabilitypeer.cc
@@ -0,0 +1,187 @@
+#include "sappeer.h"
+#include "sapconnection.h"
+#include "sapconnectionrequest.h"
+#include "sapsocket.h"
+#include "sapmanager.h"
+#include "capabilitypeer.h"
+
+CapabilityPeer::CapabilityPeer(SAPPeer *peer, QObject *parent) :
+    QObject(parent), _peer(peer), _conn(0), _socket(0)
+{
+	_consumerProfiles.insert(SAProtocol::capabilityDiscoveryProfile,
+	                         SAProtocol::capabilityDiscoveryAgentId);
+	_providerProfiles.insert(SAProtocol::capabilityDiscoveryProfile,
+	                         SAProtocol::capabilityDiscoveryAgentId);
+
+	if (_peer->role() == SAProtocol::ClientRole) {
+		_conn = _peer->createServiceConnection(SAProtocol::capabilityDiscoveryProfile,
+		                                       SAProtocol::capabilityDiscoveryProfile, SAPServiceInfo::RoleConsumer);
+		connect(_conn, SIGNAL(connected()), SLOT(handleConnected()));
+	}
+}
+
+void CapabilityPeer::requestConnection(SAPConnectionRequest *request)
+{
+	_conn = request->connection();
+	request->accept();
+}
+
+int CapabilityPeer::remoteAgentId(const QString &profile, SAPServiceInfo::Role role)
+{
+	QHash<QString, int> *profiles = profilesByRole(role);
+	if (!profiles) return -1;
+
+	return profiles->value(profile, -1);
+}
+
+SAPServiceInfo CapabilityPeer::remoteServiceInfo(int agentId) const
+{
+	return _remoteAgents.value(agentId).info;
+}
+
+void CapabilityPeer::handleConnected()
+{
+	Q_ASSERT(_conn);
+	_socket = _conn->getSocket(SAProtocol::capabilityDiscoveryChannel);
+	Q_ASSERT(_socket);
+
+	connect(_socket, SIGNAL(messageReceived()), SLOT(handleMessageReceived()));
+
+	// Send a discovery query.
+	SAProtocol::CapabilityDiscoveryQuery msg;
+
+	msg.messageType = SAProtocol::CapabilityDiscoveryMessageTypeQuery;
+	msg.queryType = 2;
+	msg.checksum = 1;
+
+	// Query for all known profiles
+	QSet<QString> profiles = SAPManager::instance()->allProfiles();
+	profiles.remove(SAProtocol::capabilityDiscoveryProfile);
+	msg.records = profiles.toList();
+
+	qDebug() << "Quering for profiles:" << msg.records;
+
+	if (msg.records.isEmpty()) {
+		// Nothing to do
+		qWarning() << "No local profiles!";
+		return;
+	}
+
+	_socket->send(SAProtocol::packCapabilityDiscoveryQuery(msg));
+}
+
+void CapabilityPeer::handleMessageReceived()
+{
+	QByteArray data = _socket->receive();
+
+	if (data.size() < 6) {
+		qWarning() << "Invalid capability message received";
+		return;
+	}
+
+	switch (data[0]) {
+	case SAProtocol::CapabilityDiscoveryMessageTypeQuery: {
+		SAProtocol::CapabilityDiscoveryQuery msg = SAProtocol::unpackCapabilityDiscoveryQuery(data);
+		SAProtocol::CapabilityDiscoveryResponse resp;
+		SAPManager *manager = SAPManager::instance();
+
+		{
+			QDebug d = qDebug();
+			d << "Queried for caps:";
+			foreach(const QString &cap, msg.records) {
+				d << cap;
+			}
+		}
+
+		qDebug() << "Got checksum" << msg.checksum;
+
+		resp.messageType = SAProtocol::CapabilityDiscoveryMessageTypeResponse;
+		resp.queryType = 3; // Why?
+		resp.checksum = 1; // We will always cause a checksum fail for now.
+
+		foreach (const QString &profile, msg.records) {
+			int agentId = manager->registeredAgentId(profile, SAPServiceInfo::RoleProvider);
+			if (agentId >= 0) {
+				const SAPServiceInfo &info = manager->serviceInfo(agentId);
+				SAProtocol::CapabilityDiscoveryProvider provider;
+				provider.name = info.friendlyName();
+				provider.uuid = 0;
+
+				SAProtocol::CapabilityDiscoveryService service;
+				service.aspVersion = info.version();
+				service.componentId = agentId;
+				service.connTimeout = info.connectionTimeout();
+				service.profile = profile;
+				service.role = info.role();
+				provider.services.append(service);
+
+				Q_ASSERT(service.role == SAPServiceInfo::RoleProvider);
+
+				resp.providers.append(provider);
+			}
+
+			agentId = manager->registeredAgentId(profile, SAPServiceInfo::RoleConsumer);
+			if (agentId >= 0) {
+				const SAPServiceInfo &info = manager->serviceInfo(agentId);
+				SAProtocol::CapabilityDiscoveryProvider provider;
+				provider.name = info.friendlyName();
+				provider.uuid = 0x1234;
+
+				SAProtocol::CapabilityDiscoveryService service;
+				service.aspVersion = info.version();
+				service.componentId = agentId;
+				service.connTimeout = info.connectionTimeout();
+				service.profile = profile;
+				service.role = info.role();
+				provider.services.append(service);
+
+				Q_ASSERT(service.role == SAPServiceInfo::RoleConsumer);
+
+				resp.providers.append(provider);
+			}
+		}
+
+		_socket->send(SAProtocol::packCapabilityDiscoveryResponse(resp));
+
+		break;
+	}
+	case SAProtocol::CapabilityDiscoveryMessageTypeResponse: {
+		SAProtocol::CapabilityDiscoveryResponse msg = SAProtocol::unpackCapabilityDiscoveryResponse(data);
+
+		foreach (const SAProtocol::CapabilityDiscoveryProvider &provider, msg.providers) {
+			foreach (const SAProtocol::CapabilityDiscoveryService &service, provider.services) {
+				RemoteAgent ragent;
+				ragent.agentId = service.componentId;
+				ragent.info.setFriendlyName(provider.name);
+				ragent.info.setProfile(service.profile);
+				ragent.info.setRole(static_cast<SAPServiceInfo::Role>(service.role));
+				ragent.info.setVersion(service.aspVersion);
+				ragent.info.setConnectionTimeout(service.connTimeout);
+
+				_remoteAgents.insert(ragent.agentId, ragent);
+
+				QHash<QString, int> *profiles = profilesByRole(ragent.info.role());
+				if (!profiles) continue;
+				profiles->insert(ragent.info.profile(), ragent.agentId);
+			}
+		}
+
+		break;
+	}
+	default:
+		qWarning() << "Unknown message to capability socket";
+		break;
+	}
+}
+
+QHash<QString, int>* CapabilityPeer::profilesByRole(SAPServiceInfo::Role role)
+{
+	switch (role) {
+	case SAPServiceInfo::RoleProvider:
+		return &_providerProfiles;
+	case SAPServiceInfo::RoleConsumer:
+		return &_consumerProfiles;
+	default:
+		return 0;
+	}
+}
diff --git a/capabilitypeer.h b/capabilitypeer.h
new file mode 100644
index 0000000..0ad6fc6
--- /dev/null
+++ b/capabilitypeer.h
@@ -0,0 +1,49 @@
+#ifndef CAPABILITYPEER_H
+#define CAPABILITYPEER_H
+
+#include <QtCore/QObject>
+#include <QtCore/QHash>
+#include <QtCore/QMap>
+
+#include "sapserviceinfo.h"
+
+class SAPPeer;
+class SAPConnection;
+class SAPConnectionRequest;
+class SAPSocket;
+
+class CapabilityPeer : public QObject
+{
+	Q_OBJECT
+public:
+	CapabilityPeer(SAPPeer *peer, QObject *parent = 0);
+
+	void requestConnection(SAPConnectionRequest *request);
+
+	int remoteAgentId(const QString &profile, SAPServiceInfo::Role role);
+	SAPServiceInfo remoteServiceInfo(int agentId) const;
+
+private:
+	QHash<QString, int>* profilesByRole(SAPServiceInfo::Role role);
+
+private slots:
+	void handleConnected();
+	void handleMessageReceived();
+
+private:
+	SAPPeer *_peer;
+	SAPConnection *_conn;
+	SAPSocket *_socket;
+
+	struct RemoteAgent {
+		int agentId;
+		SAPServiceInfo info;
+	};
+
+	QMap<int, RemoteAgent> _remoteAgents;
+
+	QHash<QString, int> _consumerProfiles;
+	QHash<QString, int> _providerProfiles;
+};
+
+#endif // CAPABILITYPEER_H
diff --git a/crc16.cc b/crc16.cc
new file mode 100644
index 0000000..f131957
--- /dev/null
+++ b/crc16.cc
@@ -0,0 +1,49 @@
+#include "crc16.h"
+
+/** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */
+uint16_t const crc16_table[256] = {
+	0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
+	0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
+	0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
+	0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
+	0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
+	0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
+	0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
+	0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
+	0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
+	0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
+	0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
+	0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
+	0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
+	0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
+	0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
+	0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
+	0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
+	0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
+	0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
+	0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
+	0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
+	0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
+	0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
+	0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
+	0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
+	0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
+	0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
+	0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
+	0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
+	0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
+	0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
+	0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
+};
+
+static inline uint16_t crc16_byte(uint16_t crc, const uint8_t data)
+{
+	return (crc >> 8) ^ crc16_table[(crc ^ data) & 0xff];
+}
+
+uint16_t crc16(uint16_t crc, uint8_t const *buffer, size_t len)
+{
+	while (len--)
+		crc = crc16_byte(crc, *buffer++);
+	return crc;
+}
diff --git a/crc16.h b/crc16.h
new file mode 100644
index 0000000..f6591e9
--- /dev/null
+++ b/crc16.h
@@ -0,0 +1,9 @@
+#ifndef CRC16_H
+#define CRC16_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+uint16_t crc16(uint16_t crc, const uint8_t *buf, size_t len);
+
+#endif // CRC16_H
diff --git a/hfpag.cc b/hfpag.cc
new file mode 100644
index 0000000..1d84c8f
--- /dev/null
+++ b/hfpag.cc
@@ -0,0 +1,78 @@
+#include <QtCore/QDebug>
+#include "hfpag.h"
+
+HfpAg::HfpAg(const QBluetoothAddress &addr, QObject *parent) :
+    QObject(parent), _socket(new QBluetoothSocket(QBluetoothSocket::RfcommSocket, this))
+{
+	connect(_socket, SIGNAL(connected()), SLOT(handleConnected()));
+	connect(_socket, SIGNAL(disconnected()), SLOT(handleDisconnected()));
+	connect(_socket, SIGNAL(readyRead()), SLOT(handleReadyRead()));
+	_socket->connectToService(addr, 7); // HFP AG
+}
+
+void HfpAg::send(const QString &cmd)
+{
+	_socket->write("\r\n", 2);
+	_socket->write(cmd.toLatin1());
+	_socket->write("\r\n", 2);
+	//qDebug() << "HFP response:" << cmd;
+}
+
+void HfpAg::handleConnected()
+{
+	qDebug() << "Connected to HFP";
+}
+
+void HfpAg::handleDisconnected()
+{
+	qDebug() << "Disconnected from HFP";
+}
+
+void HfpAg::handleReadyRead()
+{
+	_inBuf.append(_socket->readAll());
+
+	int offset = _inBuf.indexOf("\r\n");
+	while (offset >= 0) {
+		QString cmd = QString::fromLatin1(_inBuf.mid(0, offset));
+		_inBuf.remove(0, offset + 2);
+
+		handleCommand(cmd);
+
+		offset = _inBuf.indexOf("\r\n");
+	}
+}
+
+void HfpAg::handleCommand(const QString &cmd)
+{
+	//qDebug() << "HFP command:" << cmd;
+
+	if (cmd.startsWith("AT+BRSF=")) {
+		send("+BRSF: 20");
+		send("OK");
+	} else if (cmd == "AT+CIND=?") {
+		send("+CIND: (\"call\",(0,1)),(\"callsetup\",(0-3)),(\"service\",(0-1)),(\"signal\",(0-5)),(\"roam\",(0,1)),(\"battchg\",(0-5)),(\"callheld\",(0-2))");
+		send("OK");
+	} else if (cmd == "AT+CIND?") {
+		send("+CIND: 0,0,0,0,0,3,0");
+		send("OK");
+	} else if (cmd.startsWith("AT+CMER=")) {
+		send("OK");
+	} else if (cmd.startsWith("AT+CLIP=")) {
+		send("OK");
+	} else if (cmd.startsWith("AT+COPS=")) {
+		send("OK");
+	} else if (cmd.startsWith("AT+CCWA=")) {
+		send("OK");
+	} else if (cmd.startsWith("AT+BIA=")) {
+		send("OK");
+	} else if (cmd.startsWith("AT+CLCC=")) {
+		send("OK");
+	} else if (cmd.startsWith("ATD")) {
+		qDebug() << "Dialing" << cmd.mid(3);
+		send("OK");
+	} else {
+		qWarning() << "Unknown AT command: " << cmd;
+		send("+CME ERROR: 0");
+	}
+}
diff --git a/hfpag.h b/hfpag.h
new file mode 100644
index 0000000..46e326c
--- /dev/null
+++ b/hfpag.h
@@ -0,0 +1,28 @@
+#ifndef HFPAG_H
+#define HFPAG_H
+
+#include <QtConnectivity/QBluetoothSocket>
+
+QTM_USE_NAMESPACE
+
+class HfpAg : public QObject
+{
+	Q_OBJECT
+public:
+	explicit HfpAg(const QBluetoothAddress &addr, QObject *parent = 0);
+
+private:
+	void send(const QString& cmd);
+
+private slots:
+	void handleConnected();
+	void handleDisconnected();
+	void handleReadyRead();
+	void handleCommand(const QString &cmd);
+
+private:
+	QBluetoothSocket *_socket;
+	QByteArray _inBuf;
+};
+
+#endif // HFPAG_H
diff --git a/hostmanageragent.cc b/hostmanageragent.cc
new file mode 100644
index 0000000..7ed890d
--- /dev/null
+++ b/hostmanageragent.cc
@@ -0,0 +1,57 @@
+#include "sapsocket.h"
+#include "sapconnectionrequest.h"
+#include "sapserviceinfo.h"
+#include "sapchannelinfo.h"
+#include "hostmanagerconn.h"
+#include "hostmanageragent.h"
+
+static HostManagerAgent *agent = 0;
+static const QLatin1String hostmanager_profile("/system/hostmanager");
+
+HostManagerAgent::HostManagerAgent(QObject *parent)
+    : QObject(parent), _peer(0), _socket(0)
+{
+}
+
+HostManagerAgent* HostManagerAgent::instance()
+{
+	if (!agent) {
+		agent = new HostManagerAgent;
+	}
+	return agent;
+}
+
+void HostManagerAgent::peerFound(SAPPeer *peer)
+{
+	qDebug() << "Host manager peer found" << peer->peerName();
+}
+
+void HostManagerAgent::requestConnection(SAPConnectionRequest *request)
+{
+	qDebug() << "Host manager request connection from" << request->peer()->peerName();
+	SAPConnection *conn = request->connection();
+	new HostManagerConn(conn, this);
+
+	request->accept();
+}
+
+void HostManagerAgent::registerServices(SAPManager *manager)
+{
+	SAPServiceInfo service;
+	SAPChannelInfo channel;
+
+	service.setProfile(hostmanager_profile);
+	service.setFriendlyName("HostManager");
+	service.setRole(SAPServiceInfo::RoleProvider);
+	service.setVersion(1, 0);
+	service.setConnectionTimeout(0);
+
+	channel.setChannelId(103);
+	channel.setPayloadType(SAPChannelInfo::PayloadJson);
+	channel.setQoSType(SAPChannelInfo::QoSRestricted);
+	channel.setQoSDataRate(SAPChannelInfo::QoSDataRateHigh);
+	channel.setQoSPriority(SAPChannelInfo::QoSPriorityHigh);
+	service.addChannel(channel);
+
+	manager->registerServiceAgent(service, instance());
+}
diff --git a/hostmanageragent.h b/hostmanageragent.h
new file mode 100644
index 0000000..aa9e148
--- /dev/null
+++ b/hostmanageragent.h
@@ -0,0 +1,27 @@
+#ifndef HOSTMANAGER_H
+#define HOSTMANAGER_H
+
+#include <QtCore/QObject>
+#include "sappeer.h"
+#include "sapmanager.h"
+#include "sapagent.h"
+
+class HostManagerAgent : public QObject, public SAPAgent
+{
+	Q_OBJECT
+
+	explicit HostManagerAgent(QObject *parent = 0);
+
+public:
+	static HostManagerAgent * instance();
+	static void registerServices(SAPManager *manager);
+
+	void peerFound(SAPPeer *peer);
+	void requestConnection(SAPConnectionRequest *request);
+
+private:
+	SAPPeer *_peer;
+	SAPSocket *_socket;
+};
+
+#endif // HOSTMANAGER_H
diff --git a/hostmanagerconn.cc b/hostmanagerconn.cc
new file mode 100644
index 0000000..de1c61a
--- /dev/null
+++ b/hostmanagerconn.cc
@@ -0,0 +1,155 @@
+#include <QtCore/QDateTime>
+#include <QtCore/QDebug>
+#include <QtCore/QXmlStreamWriter>
+
+#include "qtjson/json.h"
+
+#include "sappeer.h"
+#include "hostmanagerconn.h"
+
+HostManagerConn::HostManagerConn(SAPConnection *conn, QObject *parent)
+    : QObject(parent), _conn(conn), _socket(conn->getSocket(103))
+{
+	connect(_conn, SIGNAL(disconnected()), SLOT(deleteLater()));
+	Q_ASSERT(_socket);
+	connect(_socket, SIGNAL(connected()), SLOT(handleConnected()));
+	connect(_socket, SIGNAL(messageReceived()), SLOT(handleMessageReceived()));
+}
+
+void HostManagerConn::sendMessage(const QString &json)
+{
+	qDebug() << "Send JSON:" << json;
+	_socket->send(QByteArray(3,0) + json.toUtf8());
+}
+
+void HostManagerConn::sendMessage(const QVariantMap &msg)
+{
+	sendMessage(QtJson::serializeStr(msg));
+}
+
+void HostManagerConn::handleMessage(const QVariantMap &msg)
+{
+	QString msgId = msg["msgId"].toString();
+	qDebug() << "Got JSON msg" << msgId;
+	if (msgId == "mgr_watch_info_res") {
+		sendMessage("{\"timestamp\":\"1407542281196=B:L>:<=LAMO\",\"type\":\"connect\",\"msgId\":\"mgr_wearable_status_req\"}");
+	} else if (msgId == "mgr_host_status_req") {
+		sendMessage(QString("{\"type\":\"connect\",\"data\":%1,\"msgId\":\"mgr_host_status_res\",\"preinstalled\":\"true\"}")
+		            .arg(QtJson::sanitizeString(generateHostXml())));
+
+	} else if (msgId == "mgr_status_exchange_done") {
+		performTimeSync();
+		sendMessage(QString("{\"btMac\":\"%1\",\"msgId\":\"mgr_setupwizard_eula_finished_req\",\"isOld\":1}")
+		            .arg(_conn->peer()->localName()));
+	}
+}
+
+void HostManagerConn::performTimeSync()
+{
+	//{"date1224":"24","datetimeepoch":"1409343828044","safety_declared":"0","locale":"es_ES","safety_voice":"1",
+	// "safetyVersion":0,"timezone":"Europe\/Madrid","safety":"false","tablet":"true","dateformat":"dd-MM-yyyy",
+	// "isfrominitial":true,"msgId":"mgr_sync_init_setting_req","usingCamera":"false","safety_cam":"0",
+	// "datetime":"2014 08 29 22 23 48","incomingCall":"false"}
+
+	QVariantMap msg;
+	msg["msgId"] = "mgr_sync_init_setting_req";
+
+	msg["safety_declared"] = "0";
+	msg["safety_voice"] = "0";
+	msg["safetyVersion"] = "0";
+	msg["safety"] = "false";
+	msg["tablet"] = "true";
+	msg["incomingCall"] = "false";
+	msg["usingCamera"] = "false";
+	msg["safety_cam"] = "0";
+
+	msg["locale"] = QLocale::system().name(); // ie es_ES
+	msg["data1224"] = "24"; // TODO
+	msg["dateformat"] = QLocale::system().dateFormat(QLocale::ShortFormat);
+	msg["timezone"] = "Europe/Madrid";
+
+	QDateTime dt = QDateTime::currentDateTime();
+	msg["datetimeepoch"] = QString::number(dt.currentMSecsSinceEpoch());
+	msg["datetime"] = dt.toString("yyyy MM dd hh mm ss");
+
+	sendMessage(msg);
+}
+
+QString HostManagerConn::generateHostXml()
+{
+	QString xml;
+	QXmlStreamWriter w(&xml);
+
+	w.setCodec("UTF-8");
+	w.setAutoFormatting(true);
+
+	w.writeStartDocument();
+
+	w.writeStartElement("DeviceStatus");
+	w.writeStartElement("device");
+	w.writeTextElement("deviceID", _conn->peer()->localName());
+	w.writeTextElement("deviceName", "none");
+	w.writeTextElement("devicePlatform", "android");
+	w.writeTextElement("devicePlatformVersion", "4.4.2");
+	w.writeTextElement("deviceType", "Host");
+	w.writeTextElement("modelNumber", "EvilPhone");
+	w.writeTextElement("swVersion", "1.0");
+
+	w.writeEmptyElement("connectivity");
+	w.writeEmptyElement("apps");
+
+	w.writeStartElement("deviceFeature");
+	w.writeTextElement("telephony", "true");
+	w.writeTextElement("messaging", "true");
+	w.writeTextElement("tablet", "true");
+	w.writeTextElement("autolock", "true");
+	w.writeTextElement("smartrelay", "true");
+	w.writeTextElement("safetyassistance", "false");
+	w.writeTextElement("vendor", "Samsung");
+	w.writeEndElement();
+
+	w.writeEmptyElement("security");
+	w.writeEmptyElement("notification");
+	w.writeEmptyElement("settings");
+
+	w.writeEndElement();
+
+	w.writeEndElement();
+
+	w.writeEndDocument();
+
+	return xml;
+}
+
+void HostManagerConn::handleConnected()
+{
+	qDebug() << "Manager socket now connected!";
+	QString msg = QString("{\"btMac\":\"%1\",\"msgId\":\"mgr_watch_info_req\",\"hmVer\":\"2.0.14041404\"}").arg(_conn->peer()->localName());
+	qDebug() << msg;
+	QByteArray data = QByteArray(3,0) + msg.toUtf8();
+	_socket->send(data);
+}
+
+void HostManagerConn::handleMessageReceived()
+{
+	QByteArray data = _socket->receive();
+
+	if (data.size() < 5) {
+		qWarning() << "Invalid HostManager message received";
+		return;
+	}
+
+	data.remove(0, 3); // First two bytes contain something related to ???
+
+	QString str = QString::fromUtf8(data);
+	bool success = false;
+	QVariant json = QtJson::parse(str, success);
+
+	if (success) {
+		qDebug() << "Got JSON:" << str;
+		handleMessage(json.toMap());
+	} else {
+		qWarning() << "Cannot parse JSON msg:" << str;
+		return;
+	}
+}
diff --git a/hostmanagerconn.h b/hostmanagerconn.h
new file mode 100644
index 0000000..ab143a0
--- /dev/null
+++ b/hostmanagerconn.h
@@ -0,0 +1,33 @@
+#ifndef HOSTMANAGERPEER_H
+#define HOSTMANAGERPEER_H
+
+#include <QtCore/QVariant>
+#include "sapconnection.h"
+#include "sapsocket.h"
+
+class HostManagerConn : public QObject
+{
+	Q_OBJECT
+
+public:
+	HostManagerConn(SAPConnection *conn, QObject *parent = 0);
+
+private:
+	void sendMessage(const QString &json);
+	void sendMessage(const QVariantMap &msg);
+	void handleMessage(const QVariantMap &msg);
+
+	void performTimeSync();
+
+	QString generateHostXml();
+
+private slots:
+	void handleConnected();
+	void handleMessageReceived();
+
+private:
+	SAPConnection *_conn;
+	SAPSocket *_socket;
+};
+
+#endif // HOSTMANAGERPEER_H
diff --git a/main.cc b/main.cc
new file mode 100644
index 0000000..fd042f3
--- /dev/null
+++ b/main.cc
@@ -0,0 +1,29 @@
+#include <QtCore/QCoreApplication>
+#include <QtCore/QStringList>
+#include "sapmanager.h"
+#include "sapbtlistener.h"
+
+#include "capabilityagent.h"
+#include "hostmanageragent.h"
+
+int main(int argc, char *argv[])
+{
+	QCoreApplication app(argc, argv);
+	app.setOrganizationName("javispedro");
+	app.setOrganizationDomain("com.javispedro");
+	app.setApplicationName("sapd");
+
+	SAPManager *manager = SAPManager::instance();
+
+	CapabilityAgent::registerServices(manager);
+	HostManagerAgent::registerServices(manager);
+
+	QScopedPointer<SAPBTListener> sap_listener(new SAPBTListener);
+
+	QBluetoothAddress address = QBluetoothAddress(app.arguments().at(1));
+
+	sap_listener->start();
+	sap_listener->nudge(address);
+
+	return app.exec();
+}
diff --git a/qtjson/json.cc b/qtjson/json.cc
new file mode 100644
index 0000000..759cf0e
--- /dev/null
+++ b/qtjson/json.cc
@@ -0,0 +1,549 @@
+/**
+ * QtJson - A simple class for parsing JSON data into a QVariant hierarchies and vice-versa.
+ * Copyright (C) 2011  Eeli Reilin
+ *
+ * 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 3 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, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * \file json.cpp
+ */
+
+#include <QDateTime>
+#include "json.h"
+
+namespace QtJson {
+    static QString dateFormat, dateTimeFormat;
+
+    static QByteArray join(const QList<QByteArray> &list, const QByteArray &sep);
+    static QVariant parseValue(const QString &json, int &index, bool &success);
+    static QVariant parseObject(const QString &json, int &index, bool &success);
+    static QVariant parseArray(const QString &json, int &index, bool &success);
+    static QVariant parseString(const QString &json, int &index, bool &success);
+    static QVariant parseNumber(const QString &json, int &index);
+    static int lastIndexOfNumber(const QString &json, int index);
+    static void eatWhitespace(const QString &json, int &index);
+    static int lookAhead(const QString &json, int index);
+    static int nextToken(const QString &json, int &index);
+
+    template<typename T>
+    QByteArray serializeMap(const T &map, bool &success) {
+        QByteArray str = "{ ";
+        QList<QByteArray> pairs;
+        for (typename T::const_iterator it = map.begin(), itend = map.end(); it != itend; ++it) {
+            QByteArray serializedValue = serialize(it.value());
+            if (serializedValue.isNull()) {
+                success = false;
+                break;
+            }
+            pairs << sanitizeString(it.key()).toUtf8() + " : " + serializedValue;
+        }
+
+        str += join(pairs, ", ");
+        str += " }";
+        return str;
+    }
+
+
+    /**
+     * parse
+     */
+    QVariant parse(const QString &json) {
+        bool success = true;
+        return parse(json, success);
+    }
+
+    /**
+     * parse
+     */
+    QVariant parse(const QString &json, bool &success) {
+        success = true;
+
+        // Return an empty QVariant if the JSON data is either null or empty
+        if (!json.isNull() || !json.isEmpty()) {
+            QString data = json;
+            // We'll start from index 0
+            int index = 0;
+
+            // Parse the first value
+            QVariant value = parseValue(data, index, success);
+
+            // Return the parsed value
+            return value;
+        } else {
+            // Return the empty QVariant
+            return QVariant();
+        }
+    }
+
+    QByteArray serialize(const QVariant &data) {
+        bool success = true;
+        return serialize(data, success);
+    }
+
+    QByteArray serialize(const QVariant &data, bool &success) {
+        QByteArray str;
+        success = true;
+
+        if (!data.isValid()) { // invalid or null?
+            str = "null";
+        } else if ((data.type() == QVariant::List) ||
+                   (data.type() == QVariant::StringList)) { // variant is a list?
+            QList<QByteArray> values;
+            const QVariantList list = data.toList();
+            Q_FOREACH(const QVariant& v, list) {
+                QByteArray serializedValue = serialize(v);
+                if (serializedValue.isNull()) {
+                    success = false;
+                    break;
+                }
+                values << serializedValue;
+            }
+
+            str = "[ " + join( values, ", " ) + " ]";
+        } else if (data.type() == QVariant::Hash) { // variant is a hash?
+            str = serializeMap<>(data.toHash(), success);
+        } else if (data.type() == QVariant::Map) { // variant is a map?
+            str = serializeMap<>(data.toMap(), success);
+        } else if ((data.type() == QVariant::String) ||
+                   (data.type() == QVariant::ByteArray)) {// a string or a byte array?
+            str = sanitizeString(data.toString()).toUtf8();
+        } else if (data.type() == QVariant::Double) { // double?
+            double value = data.toDouble();
+            if ((value - value) == 0.0) {
+                str = QByteArray::number(value, 'g');
+                if (!str.contains(".") && ! str.contains("e")) {
+                    str += ".0";
+                }
+            } else {
+                success = false;
+            }
+        } else if (data.type() == QVariant::Bool) { // boolean value?
+            str = data.toBool() ? "true" : "false";
+        } else if (data.type() == QVariant::ULongLong) { // large unsigned number?
+            str = QByteArray::number(data.value<qulonglong>());
+        } else if (data.canConvert<qlonglong>()) { // any signed number?
+            str = QByteArray::number(data.value<qlonglong>());
+        } else if (data.canConvert<long>()) { //TODO: this code is never executed because all smaller types can be converted to qlonglong
+            str = QString::number(data.value<long>()).toUtf8();
+        } else if (data.type() == QVariant::DateTime) { // datetime value?
+            str = sanitizeString(dateTimeFormat.isEmpty()
+                                 ? data.toDateTime().toString()
+                                 : data.toDateTime().toString(dateTimeFormat)).toUtf8();
+        } else if (data.type() == QVariant::Date) { // date value?
+            str = sanitizeString(dateTimeFormat.isEmpty()
+                                 ? data.toDate().toString()
+                                 : data.toDate().toString(dateFormat)).toUtf8();
+        } else if (data.canConvert<QString>()) { // can value be converted to string?
+            // this will catch QUrl, ... (all other types which can be converted to string)
+            str = sanitizeString(data.toString()).toUtf8();
+        } else {
+            success = false;
+        }
+
+        if (success) {
+            return str;
+        } else {
+            return QByteArray();
+        }
+    }
+
+    QString serializeStr(const QVariant &data) {
+        return QString::fromUtf8(serialize(data));
+    }
+
+    QString serializeStr(const QVariant &data, bool &success) {
+        return QString::fromUtf8(serialize(data, success));
+    }
+
+
+    /**
+     * \enum JsonToken
+     */
+    enum JsonToken {
+        JsonTokenNone = 0,
+        JsonTokenCurlyOpen = 1,
+        JsonTokenCurlyClose = 2,
+        JsonTokenSquaredOpen = 3,
+        JsonTokenSquaredClose = 4,
+        JsonTokenColon = 5,
+        JsonTokenComma = 6,
+        JsonTokenString = 7,
+        JsonTokenNumber = 8,
+        JsonTokenTrue = 9,
+        JsonTokenFalse = 10,
+        JsonTokenNull = 11
+    };
+
+    QString sanitizeString(QString str) {
+        str.replace(QLatin1String("\\"), QLatin1String("\\\\"));
+        str.replace(QLatin1String("\""), QLatin1String("\\\""));
+        str.replace(QLatin1String("\b"), QLatin1String("\\b"));
+        str.replace(QLatin1String("\f"), QLatin1String("\\f"));
+        str.replace(QLatin1String("\n"), QLatin1String("\\n"));
+        str.replace(QLatin1String("\r"), QLatin1String("\\r"));
+        str.replace(QLatin1String("\t"), QLatin1String("\\t"));
+        return QString(QLatin1String("\"%1\"")).arg(str);
+    }
+
+    static QByteArray join(const QList<QByteArray> &list, const QByteArray &sep) {
+        QByteArray res;
+        Q_FOREACH(const QByteArray &i, list) {
+            if (!res.isEmpty()) {
+                res += sep;
+            }
+            res += i;
+        }
+        return res;
+    }
+
+    /**
+     * parseValue
+     */
+    static QVariant parseValue(const QString &json, int &index, bool &success) {
+        // Determine what kind of data we should parse by
+        // checking out the upcoming token
+        switch(lookAhead(json, index)) {
+            case JsonTokenString:
+                return parseString(json, index, success);
+            case JsonTokenNumber:
+                return parseNumber(json, index);
+            case JsonTokenCurlyOpen:
+                return parseObject(json, index, success);
+            case JsonTokenSquaredOpen:
+                return parseArray(json, index, success);
+            case JsonTokenTrue:
+                nextToken(json, index);
+                return QVariant(true);
+            case JsonTokenFalse:
+                nextToken(json, index);
+                return QVariant(false);
+            case JsonTokenNull:
+                nextToken(json, index);
+                return QVariant();
+            case JsonTokenNone:
+                break;
+        }
+
+        // If there were no tokens, flag the failure and return an empty QVariant
+        success = false;
+        return QVariant();
+    }
+
+    /**
+     * parseObject
+     */
+    static QVariant parseObject(const QString &json, int &index, bool &success) {
+        QVariantMap map;
+        int token;
+
+        // Get rid of the whitespace and increment index
+        nextToken(json, index);
+
+        // Loop through all of the key/value pairs of the object
+        bool done = false;
+        while (!done) {
+            // Get the upcoming token
+            token = lookAhead(json, index);
+
+            if (token == JsonTokenNone) {
+                success = false;
+                return QVariantMap();
+            } else if (token == JsonTokenComma) {
+                nextToken(json, index);
+            } else if (token == JsonTokenCurlyClose) {
+                nextToken(json, index);
+                return map;
+            } else {
+                // Parse the key/value pair's name
+                QString name = parseString(json, index, success).toString();
+
+                if (!success) {
+                    return QVariantMap();
+                }
+
+                // Get the next token
+                token = nextToken(json, index);
+
+                // If the next token is not a colon, flag the failure
+                // return an empty QVariant
+                if (token != JsonTokenColon) {
+                    success = false;
+                    return QVariant(QVariantMap());
+                }
+
+                // Parse the key/value pair's value
+                QVariant value = parseValue(json, index, success);
+
+                if (!success) {
+                    return QVariantMap();
+                }
+
+                // Assign the value to the key in the map
+                map[name] = value;
+            }
+        }
+
+        // Return the map successfully
+        return QVariant(map);
+    }
+
+    /**
+     * parseArray
+     */
+    static QVariant parseArray(const QString &json, int &index, bool &success) {
+        QVariantList list;
+
+        nextToken(json, index);
+
+        bool done = false;
+        while(!done) {
+            int token = lookAhead(json, index);
+
+            if (token == JsonTokenNone) {
+                success = false;
+                return QVariantList();
+            } else if (token == JsonTokenComma) {
+                nextToken(json, index);
+            } else if (token == JsonTokenSquaredClose) {
+                nextToken(json, index);
+                break;
+            } else {
+                QVariant value = parseValue(json, index, success);
+                if (!success) {
+                    return QVariantList();
+                }
+                list.push_back(value);
+            }
+        }
+
+        return QVariant(list);
+    }
+
+    /**
+     * parseString
+     */
+    static QVariant parseString(const QString &json, int &index, bool &success) {
+        QString s;
+        QChar c;
+
+        eatWhitespace(json, index);
+
+        c = json[index++];
+
+        bool complete = false;
+        while(!complete) {
+            if (index == json.size()) {
+                break;
+            }
+
+            c = json[index++];
+
+            if (c == '\"') {
+                complete = true;
+                break;
+            } else if (c == '\\') {
+                if (index == json.size()) {
+                    break;
+                }
+
+                c = json[index++];
+
+                if (c == '\"') {
+                    s.append('\"');
+                } else if (c == '\\') {
+                    s.append('\\');
+                } else if (c == '/') {
+                    s.append('/');
+                } else if (c == 'b') {
+                    s.append('\b');
+                } else if (c == 'f') {
+                    s.append('\f');
+                } else if (c == 'n') {
+                    s.append('\n');
+                } else if (c == 'r') {
+                    s.append('\r');
+                } else if (c == 't') {
+                    s.append('\t');
+                } else if (c == 'u') {
+                    int remainingLength = json.size() - index;
+                    if (remainingLength >= 4) {
+                        QString unicodeStr = json.mid(index, 4);
+
+                        int symbol = unicodeStr.toInt(0, 16);
+
+                        s.append(QChar(symbol));
+
+                        index += 4;
+                    } else {
+                        break;
+                    }
+                }
+            } else {
+                s.append(c);
+            }
+        }
+
+        if (!complete) {
+            success = false;
+            return QVariant();
+        }
+
+        return QVariant(s);
+    }
+
+    /**
+     * parseNumber
+     */
+    static QVariant parseNumber(const QString &json, int &index) {
+        eatWhitespace(json, index);
+
+        int lastIndex = lastIndexOfNumber(json, index);
+        int charLength = (lastIndex - index) + 1;
+        QString numberStr;
+
+        numberStr = json.mid(index, charLength);
+
+        index = lastIndex + 1;
+        bool ok;
+
+        if (numberStr.contains('.')) {
+            return QVariant(numberStr.toDouble(NULL));
+        } else if (numberStr.startsWith('-')) {
+            int i = numberStr.toInt(&ok);
+            if (!ok) {
+                qlonglong ll = numberStr.toLongLong(&ok);
+                return ok ? ll : QVariant(numberStr);
+            }
+            return i;
+        } else {
+            uint u = numberStr.toUInt(&ok);
+            if (!ok) {
+                qulonglong ull = numberStr.toULongLong(&ok);
+                return ok ? ull : QVariant(numberStr);
+            }
+            return u;
+        }
+    }
+
+    /**
+     * lastIndexOfNumber
+     */
+    static int lastIndexOfNumber(const QString &json, int index) {
+        int lastIndex;
+
+        for(lastIndex = index; lastIndex < json.size(); lastIndex++) {
+            if (QString("0123456789+-.eE").indexOf(json[lastIndex]) == -1) {
+                break;
+            }
+        }
+
+        return lastIndex -1;
+    }
+
+    /**
+     * eatWhitespace
+     */
+    static void eatWhitespace(const QString &json, int &index) {
+        for(; index < json.size(); index++) {
+            if (QString(" \t\n\r").indexOf(json[index]) == -1) {
+                break;
+            }
+        }
+    }
+
+    /**
+     * lookAhead
+     */
+    static int lookAhead(const QString &json, int index) {
+        int saveIndex = index;
+        return nextToken(json, saveIndex);
+    }
+
+    /**
+     * nextToken
+     */
+    static int nextToken(const QString &json, int &index) {
+        eatWhitespace(json, index);
+
+        if (index == json.size()) {
+            return JsonTokenNone;
+        }
+
+        QChar c = json[index];
+        index++;
+        switch(c.toLatin1()) {
+            case '{': return JsonTokenCurlyOpen;
+            case '}': return JsonTokenCurlyClose;
+            case '[': return JsonTokenSquaredOpen;
+            case ']': return JsonTokenSquaredClose;
+            case ',': return JsonTokenComma;
+            case '"': return JsonTokenString;
+            case '0': case '1': case '2': case '3': case '4':
+            case '5': case '6': case '7': case '8': case '9':
+            case '-': return JsonTokenNumber;
+            case ':': return JsonTokenColon;
+        }
+        index--; // ^ WTF?
+
+        int remainingLength = json.size() - index;
+
+        // True
+        if (remainingLength >= 4) {
+            if (json[index] == 't' && json[index + 1] == 'r' &&
+                json[index + 2] == 'u' && json[index + 3] == 'e') {
+                index += 4;
+                return JsonTokenTrue;
+            }
+        }
+
+        // False
+        if (remainingLength >= 5) {
+            if (json[index] == 'f' && json[index + 1] == 'a' &&
+                json[index + 2] == 'l' && json[index + 3] == 's' &&
+                json[index + 4] == 'e') {
+                index += 5;
+                return JsonTokenFalse;
+            }
+        }
+
+        // Null
+        if (remainingLength >= 4) {
+            if (json[index] == 'n' && json[index + 1] == 'u' &&
+                json[index + 2] == 'l' && json[index + 3] == 'l') {
+                index += 4;
+                return JsonTokenNull;
+            }
+        }
+
+        return JsonTokenNone;
+    }
+
+    void setDateTimeFormat(const QString &format) {
+        dateTimeFormat = format;
+    }
+
+    void setDateFormat(const QString &format) {
+        dateFormat = format;
+    }
+
+    QString getDateTimeFormat() {
+        return dateTimeFormat;
+    }
+
+    QString getDateFormat() {
+        return dateFormat;
+    }
+
+} //end namespace
diff --git a/qtjson/json.h b/qtjson/json.h
new file mode 100644
index 0000000..5482b6a
--- /dev/null
+++ b/qtjson/json.h
@@ -0,0 +1,112 @@
+/**
+ * QtJson - A simple class for parsing JSON data into a QVariant hierarchies and vice-versa.
+ * Copyright (C) 2011  Eeli Reilin
+ *
+ * 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 3 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, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * \file json.h
+ */
+
+#ifndef JSON_H
+#define JSON_H
+
+#include <QVariant>
+#include <QString>
+
+
+/**
+ * \namespace QtJson
+ * \brief A JSON data parser
+ *
+ * Json parses a JSON data into a QVariant hierarchy.
+ */
+namespace QtJson {
+    typedef QVariantMap JsonObject;
+    typedef QVariantList JsonArray;
+
+    /**
+     * Parse a JSON string
+     *
+     * \param json The JSON data
+     */
+    QVariant parse(const QString &json);
+
+    /**
+     * Parse a JSON string
+     *
+     * \param json The JSON data
+     * \param success The success of the parsing
+     */
+    QVariant parse(const QString &json, bool &success);
+
+    /**
+     * This method generates a textual JSON representation
+     *
+     * \param data The JSON data generated by the parser.
+     *
+     * \return QByteArray Textual JSON representation in UTF-8
+     */
+    QByteArray serialize(const QVariant &data);
+
+    /**
+     * This method generates a textual JSON representation
+     *
+     * \param data The JSON data generated by the parser.
+     * \param success The success of the serialization
+     *
+     * \return QByteArray Textual JSON representation in UTF-8
+     */
+    QByteArray serialize(const QVariant &data, bool &success);
+
+    /**
+     * This method generates a textual JSON representation
+     *
+     * \param data The JSON data generated by the parser.
+     *
+     * \return QString Textual JSON representation
+     */
+    QString serializeStr(const QVariant &data);
+
+    /**
+     * This method generates a textual JSON representation
+     *
+     * \param data The JSON data generated by the parser.
+     * \param success The success of the serialization
+     *
+     * \return QString Textual JSON representation
+     */
+    QString serializeStr(const QVariant &data, bool &success);
+
+    /**
+     * This method sets date(time) format to be used for QDateTime::toString
+     * If QString is empty, Qt::TextDate is used.
+     *
+     * \param format The JSON data generated by the parser.
+     */
+    void setDateTimeFormat(const QString& format);
+    void setDateFormat(const QString& format);
+
+    /**
+     * This method gets date(time) format to be used for QDateTime::toString
+     * If QString is empty, Qt::TextDate is used.
+     */
+    QString getDateTimeFormat();
+    QString getDateFormat();
+
+	QString sanitizeString(QString str);
+}
+
+#endif //JSON_H
diff --git a/sapagent.h b/sapagent.h
new file mode 100644
index 0000000..321075c
--- /dev/null
+++ b/sapagent.h
@@ -0,0 +1,17 @@
+#ifndef SAAGENT_H
+#define SAAGENT_H
+
+#include <QtCore/QString>
+
+class SAPSocket;
+class SAPPeer;
+class SAPConnectionRequest;
+
+class SAPAgent
+{
+public:
+	virtual void peerFound(SAPPeer *peer) = 0;
+	virtual void requestConnection(SAPConnectionRequest *request) = 0;
+};
+
+#endif // SAAGENT_H
diff --git a/sapbtlistener.cc b/sapbtlistener.cc
new file mode 100644
index 0000000..d5cd071
--- /dev/null
+++ b/sapbtlistener.cc
@@ -0,0 +1,198 @@
+#include <QtCore/QDebug>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+#include "saprotocol.h"
+#include "sapbtlistener.h"
+#include "sapbtpeer.h"
+
+#ifdef DESKTOP
+#include "hfpag.h"
+#endif
+
+namespace
+{
+void add_sdp_record(sdp_session_t *session, const QBluetoothUuid &btuuid, quint16 port)
+{
+	sdp_list_t *svclass_id, *apseq, *root;
+	uuid_t root_uuid, svclass_uuid, l2cap_uuid, rfcomm_uuid;
+	sdp_list_t *aproto, *proto[2];
+	sdp_data_t *chan;
+
+	sdp_record_t *record = sdp_record_alloc();
+
+	sdp_set_info_attr(record, "SAP", "gearbttest", "Samsung Accessory Protocol");
+
+	sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
+	root = sdp_list_append(0, &root_uuid);
+	sdp_set_browse_groups(record, root);
+
+	QByteArray uuid = btuuid.toRfc4122();
+	sdp_uuid128_create(&svclass_uuid, uuid.constData());
+	svclass_id = sdp_list_append(0, &svclass_uuid);
+	sdp_set_service_classes(record, svclass_id);
+
+	sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID);
+	proto[0] = sdp_list_append(0, &l2cap_uuid);
+	apseq = sdp_list_append(0, proto[0]);
+
+	sdp_uuid16_create(&rfcomm_uuid, RFCOMM_UUID);
+	proto[1] = sdp_list_append(0, &rfcomm_uuid);
+	chan = sdp_data_alloc(SDP_UINT8, &port);
+	proto[1] = sdp_list_append(proto[1], chan);
+	apseq = sdp_list_append(apseq, proto[1]);
+
+	aproto = sdp_list_append(0, apseq);
+	sdp_set_access_protos(record, aproto);
+
+	if (sdp_record_register(session, record, 0) < 0) {
+		qWarning() << "sdp_record_register failed";
+	}
+}
+
+void add_sdp_records(quint16 port)
+{
+	bdaddr_t addr_any = {{0, 0, 0, 0, 0, 0}};
+	bdaddr_t addr_local = {{0, 0, 0, 0xFF, 0xFF, 0xFF}};
+
+	sdp_session_t *sess = sdp_connect(&addr_any, &addr_local, 0);
+	if (!sess) {
+		qWarning() << "sdp_connect failed";
+		return;
+	}
+
+	add_sdp_record(sess, SAProtocol::dataServiceUuid, port);
+}
+
+}
+
+SAPBTListener::SAPBTListener(QObject *parent) :
+    QObject(parent), _server(0)
+{
+}
+
+SAPBTListener::~SAPBTListener()
+{
+	stop();
+}
+
+void SAPBTListener::start()
+{
+	if (_server) {
+		return;
+	}
+
+	_server = new QRfcommServer(this);
+	connect(_server, SIGNAL(newConnection()), this, SLOT(acceptConnection()));
+	if (!_server->listen(QBluetoothAddress(), 0)) {
+		qWarning() << "Failed to start Bluetooth listener socket";
+		stop();
+		return;
+	}
+
+	quint8 serverPort = _server->serverPort();
+
+#if 1
+	// Basically, QBluetoothServiceInfo is not yet compatible with Bluez5,
+	// so we hack around it by doing our own SDP connection.
+	add_sdp_records(serverPort);
+#else
+
+	/*
+	 	Service Name: SAP
+		Service RecHandle: 0x1000b
+		Service Class ID List:
+		  UUID 128: a49eb41e-cb06-495c-9f4f-bb80a90cdf00
+		Protocol Descriptor List:
+		  "L2CAP" (0x0100)
+		  "RFCOMM" (0x0003)
+			Channel: 5
+
+		Service Name: SAP
+		Service RecHandle: 0x1000c
+		Service Class ID List:
+		  UUID 128: a49eb41e-cb06-495c-9f4f-aa80a90cdf4a
+		Protocol Descriptor List:
+		  "L2CAP" (0x0100)
+		  "RFCOMM" (0x0003)
+			Channel: 6
+ */
+
+	_service.setServiceName("SAP");
+	_service.setServiceDescription("Samsung Accessory Profile");
+	_service.setServiceProvider("gearbtteest");
+
+	QBluetoothServiceInfo::Sequence classIds;
+	classIds.append(QVariant::fromValue(SAPProtocol::dataServiceUuid));
+	_service.setAttribute(QBluetoothServiceInfo::ServiceClassIds, classIds);
+
+	QBluetoothServiceInfo::Sequence browseGroupList;
+	browseGroupList.append(QVariant::fromValue(QBluetoothUuid(QBluetoothUuid::PublicBrowseGroup)));
+	_service.setAttribute(QBluetoothServiceInfo::BrowseGroupList, browseGroupList);
+
+	QBluetoothServiceInfo::Sequence protocolDescriptorList;
+	QBluetoothServiceInfo::Sequence protocol;
+
+	protocol.append(QVariant::fromValue(QBluetoothUuid(QBluetoothUuid::L2cap)));
+	protocolDescriptorList.append(QVariant::fromValue(protocol));
+	protocol.clear();
+
+	protocol.append(QVariant::fromValue(QBluetoothUuid(QBluetoothUuid::Rfcomm)));
+	protocol.append(QVariant::fromValue(serverPort));
+	protocolDescriptorList.append(QVariant::fromValue(protocol));
+	protocol.clear();
+
+	_service.setAttribute(QBluetoothServiceInfo::ProtocolDescriptorList,
+	                       protocolDescriptorList);
+
+	if (!_service.registerService()) {
+		qWarning() << "Failed to register the SAP service";
+	}
+#endif
+}
+
+void SAPBTListener::stop()
+{
+	if (!_server) {
+		return;
+	}
+
+	if (!_service.unregisterService()) {
+		qWarning() << "Failed to unregister SAP service";
+	}
+
+	delete _server;
+	_server = 0;
+}
+
+void SAPBTListener::nudge(const QBluetoothAddress &address)
+{
+	QBluetoothSocket *socket = new QBluetoothSocket(QBluetoothSocket::RfcommSocket, this);
+
+	qDebug() << "Nudging" << address.toString();
+
+	socket->connectToService(address, 2); //SAPProtocol::nudgeServiceUuid);
+
+#if 1
+	HfpAg *ag = new HfpAg(address, this);
+	connect(socket, SIGNAL(disconnected()), ag, SLOT(deleteLater()));
+#endif
+
+	connect(socket, SIGNAL(connected()), socket, SLOT(deleteLater()));
+	connect(socket, SIGNAL(error(QBluetoothSocket::SocketError)), socket, SLOT(deleteLater()));
+}
+
+void SAPBTListener::acceptConnection()
+{
+	qDebug() << "Incoming BT connection";
+	QBluetoothSocket *socket = _server->nextPendingConnection();
+	if (!socket) {
+		qWarning() << "Actually, no incoming connection";
+		return;
+	}
+
+	qDebug() << "Got connection";
+
+	// TODO Why am I hardcoding the role here
+	new SAPBTPeer(SAProtocol::ClientRole, socket, this);
+}
diff --git a/sapbtlistener.h b/sapbtlistener.h
new file mode 100644
index 0000000..ee2fb7e
--- /dev/null
+++ b/sapbtlistener.h
@@ -0,0 +1,42 @@
+#ifndef SAPBTLISTENER_H
+#define SAPBTLISTENER_H
+
+#include <QtCore/QObject>
+
+#if QT_VERSION >= QT_VERSION_CHECK(5,0,0)
+#include <QtBluetooth/QRfcommServer>
+#include <QtBluetooth/QBluetoothServiceInfo>
+QT_USE_NAMESPACE_BLUETOOTH
+#else
+#include <QtConnectivity/QRfcommServer>
+#include <QtConnectivity/QBluetoothServiceInfo>
+QTM_USE_NAMESPACE
+#endif
+
+class SAPBTListener : public QObject
+{
+	Q_OBJECT
+public:
+	explicit SAPBTListener(QObject *parent = 0);
+	~SAPBTListener();
+
+signals:
+
+public slots:
+	void start();
+	void stop();
+
+	void nudge(const QBluetoothAddress &address);
+
+private:
+
+private slots:
+	void acceptConnection();
+
+
+private:
+	QRfcommServer *_server;
+	QBluetoothServiceInfo _service;
+};
+
+#endif // SAPBTLISTENER_H
diff --git a/sapbtpeer.cc b/sapbtpeer.cc
new file mode 100644
index 0000000..9fc56af
--- /dev/null
+++ b/sapbtpeer.cc
@@ -0,0 +1,181 @@
+#include <QtEndian>
+#include "saprotocol.h"
+#include "wmspeer.h"
+#include "crc16.h"
+#include "sapsocket.h"
+#include "sapbtpeer.h"
+
+#define PROTO_DEBUG 0
+
+SAPBTPeer::SAPBTPeer(SAProtocol::Role role, QBluetoothSocket *socket, QObject *parent) :
+    SAPPeer(role, socket->localAddress().toString(), socket->peerAddress().toString(), parent),
+    _socket(socket),
+    _curFrameLength(0),
+    _peerDescriptionExchangeDone(false), _authenticationDone(false)
+{
+	connect(_socket, SIGNAL(readyRead()), SLOT(handleSocketData()));
+	connect(_socket, SIGNAL(disconnected()), SLOT(handleSocketDisconnected()));
+}
+
+void SAPBTPeer::handleSocketData()
+{
+	uint bytes = _socket->bytesAvailable();
+	const bool need_crc = _peerDescriptionExchangeDone && _authenticationDone;
+	const uint header_size = need_crc ? 2 * sizeof(quint16) : 1 * sizeof(quint16);
+	const uint footer_size = need_crc ? sizeof(quint16) : 0;
+
+	while ((_curFrameLength == 0 && bytes >= header_size) ||
+	       (_curFrameLength > 0 && bytes >= _curFrameLength + footer_size)) {
+		if (_curFrameLength > 0) {
+			QByteArray frame = _socket->read(_curFrameLength);
+			Q_ASSERT(frame.size() == (int)_curFrameLength);
+			_curFrameLength = 0;
+
+			if (need_crc) {
+				quint16 computed_crc = crc16(0, reinterpret_cast<const quint8*>(frame.constData()), frame.size());
+				quint16 crc;
+				_socket->read(reinterpret_cast<char*>(&crc), sizeof(quint16));
+				crc = qFromBigEndian(crc);
+
+				if (crc != computed_crc) {
+					qWarning() << "CRC data failure";
+					_socket->close(); // Drop the connection, no provision for resync.
+					return;
+				}
+			}
+
+			handleFrame(frame);
+		} else {
+			quint16 frame_length;
+			bytes = _socket->read(reinterpret_cast<char*>(&frame_length), sizeof(quint16));
+			Q_ASSERT(bytes == sizeof(quint16));
+			_curFrameLength = qFromBigEndian(frame_length);
+			Q_ASSERT(_curFrameLength > 0);
+
+			if (need_crc) {
+				// Compute the checksum of the BIG ENDIAN frame length.
+				quint16 computed_crc = crc16(0, reinterpret_cast<quint8*>(&frame_length), sizeof(quint16));
+				quint16 crc;
+				_socket->read(reinterpret_cast<char*>(&crc), sizeof(quint16));
+				crc = qFromBigEndian(crc);
+
+				if (crc != computed_crc) {
+					qWarning() << "CRC length failure";
+					_curFrameLength = 0;
+					_socket->close(); // Drop the connection, no provision for resync.
+					return;
+				}
+			}
+		}
+
+		bytes = _socket->bytesAvailable();
+	}
+}
+
+void SAPBTPeer::handleSocketDisconnected()
+{
+	qDebug() << "Socket disconnected";
+	handleDisconnected();
+}
+
+void SAPBTPeer::sendFrame(const QByteArray &data)
+{
+	const bool need_crc = _peerDescriptionExchangeDone && _authenticationDone;
+	quint16 frame_length = qToBigEndian<quint16>(data.length());
+	_socket->write(reinterpret_cast<const char*>(&frame_length), sizeof(quint16));
+
+	// Compute the checksum of the BIG ENDIAN frame length.
+	if (need_crc) {
+		quint16 crc = qToBigEndian(crc16(0, reinterpret_cast<quint8*>(&frame_length), sizeof(quint16)));
+		_socket->write(reinterpret_cast<const char*>(&crc), sizeof(quint16));
+	}
+
+	_socket->write(data.constData(), data.size());
+
+	if (need_crc) {
+		quint16 crc = qToBigEndian(crc16(0, reinterpret_cast<const quint8*>(data.constData()), data.size()));
+		_socket->write(reinterpret_cast<const char*>(&crc), sizeof(quint16));
+	}
+
+#if PROTO_DEBUG
+	qDebug() << "Sent:" << data.toHex();
+#endif
+}
+
+void SAPBTPeer::handleFrame(const QByteArray &data)
+{
+#if PROTO_DEBUG
+	qDebug() << "Recv:" << data.toHex();
+#endif
+
+	if (!_peerDescriptionExchangeDone) {
+		// This must be a peer description frame!
+		SAProtocol::PeerDescription peerDesc = SAProtocol::unpackPeerDescription(data);
+		qDebug() << peerDesc.product << peerDesc.manufacturer << peerDesc.name;
+
+		SAProtocol::PeerDescription myDesc = peerDesc;
+		myDesc.messageType = 6; // Why?
+		myDesc.status = 0; // This seems to be "accepted"
+		myDesc.product = "RandomPhone";
+		myDesc.manufacturer = "me";
+		myDesc.name = "gearbttest";
+		myDesc.profile = "SWatch"; // This is what Gear manager sends
+
+		sendFrame(SAProtocol::packPeerDescription(myDesc));
+
+		_peerDescriptionExchangeDone = true;
+	} else if (!_authenticationDone) {
+		// This must be a authentication frame...
+		handleAuthenticationFrame(data);
+	} else {
+		SAProtocol::Frame frame = SAProtocol::unpackFrame(data);
+		switch (frame.type) {
+		case SAProtocol::FrameData:
+			handleDataFrame(frame);
+			break;
+		case SAProtocol::FrameControl:
+			qWarning() << "Got control frame, what to do?";
+			break;
+		default:
+			qWarning() << "Unknown frame type" << frame.type;
+			break;
+		}
+	}
+}
+
+void SAPBTPeer::handleDataFrame(const SAProtocol::Frame &frame)
+{
+	Q_ASSERT(frame.type == SAProtocol::FrameData);
+
+	handleSessionData(frame.sessionId, frame.data);
+}
+
+void SAPBTPeer::handleAuthenticationFrame(const QByteArray &data)
+{
+	SAProtocol::SecurityFrame sframe = SAProtocol::unpackSecurityFrame(data);
+
+	switch (sframe.type) {
+	case SAProtocol::SecurityAuthenticateRequest: {
+		qDebug() << "Starting authorization...";
+		SAProtocol::SecurityFrame response = _wms->respondToServerChallenge(sframe);
+		if (!response.data.isEmpty()) {
+			sendFrame(SAProtocol::packSecurityFrame(response));
+		}
+		break;
+	}
+	case SAProtocol::SecurityAuthenticateConfirm: {
+		_authenticationDone = _wms->verifyServerResponse(sframe);
+		if (_authenticationDone) {
+			qDebug() << "Authentication confirmed";
+			handleConnected();
+		} else {
+			qWarning() << "Authentication failure, closing connection";
+			_socket->close(); // Will call "handleDisconnected" and emit disconnected signals.
+		}
+		break;
+	}
+	default:
+		qWarning() << "Unknown security frame type" << sframe.type;
+		break;
+	}
+}
diff --git a/sapbtpeer.h b/sapbtpeer.h
new file mode 100644
index 0000000..cc060f8
--- /dev/null
+++ b/sapbtpeer.h
@@ -0,0 +1,42 @@
+#ifndef SAPBTPEER_H
+#define SAPBTPEER_H
+
+#include <QtCore/QObject>
+
+#if QT_VERSION >= QT_VERSION_CHECK(5,0,0)
+#include <QtBluetooth/QBluetoothSocket>
+QT_USE_NAMESPACE_BLUETOOTH
+#else
+#include <QtConnectivity/QBluetoothSocket>
+QTM_USE_NAMESPACE
+#endif
+
+#include "sappeer.h"
+
+class SAPBTPeer : public SAPPeer
+{
+	Q_OBJECT
+
+public:
+	SAPBTPeer(SAProtocol::Role role, QBluetoothSocket *socket, QObject *parent = 0);
+
+protected:
+	void sendFrame(const QByteArray &data);
+
+private slots:
+	void handleSocketData();
+	void handleSocketDisconnected();
+
+private:
+	void handleFrame(const QByteArray &data);
+	void handleDataFrame(const SAProtocol::Frame &frame);
+	void handleAuthenticationFrame(const QByteArray &data);
+
+private:
+	QBluetoothSocket *_socket;
+	uint _curFrameLength;
+	bool _peerDescriptionExchangeDone : 1;
+	bool _authenticationDone : 1;
+};
+
+#endif // SAPBTPEER_H
diff --git a/sapchannelinfo.cc b/sapchannelinfo.cc
new file mode 100644
index 0000000..12ba5b5
--- /dev/null
+++ b/sapchannelinfo.cc
@@ -0,0 +1,80 @@
+#include "sapchannelinfo.h"
+#include <QSharedData>
+
+struct SAPChannelInfoData : public QSharedData {
+	unsigned short id;
+	SAPChannelInfo::PayloadType payload;
+	SAPChannelInfo::QoSType qosType;
+	SAPChannelInfo::QoSPriority qosPriority;
+	SAPChannelInfo::QoSDataRate qosDataRate;
+};
+
+SAPChannelInfo::SAPChannelInfo() : data(new SAPChannelInfoData)
+{
+}
+
+SAPChannelInfo::SAPChannelInfo(const SAPChannelInfo &rhs) : data(rhs.data)
+{
+}
+
+SAPChannelInfo &SAPChannelInfo::operator=(const SAPChannelInfo &rhs)
+{
+	if (this != &rhs)
+		data.operator=(rhs.data);
+	return *this;
+}
+
+SAPChannelInfo::~SAPChannelInfo()
+{
+
+}
+
+unsigned short SAPChannelInfo::channelId() const
+{
+	return data->id;
+}
+
+void SAPChannelInfo::setChannelId(unsigned short id)
+{
+	data->id = id;
+}
+
+SAPChannelInfo::PayloadType SAPChannelInfo::payloadType() const
+{
+	return data->payload;
+}
+
+void SAPChannelInfo::setPayloadType(PayloadType type)
+{
+	data->payload = type;
+}
+
+SAPChannelInfo::QoSType SAPChannelInfo::qosType() const
+{
+	return data->qosType;
+}
+
+void SAPChannelInfo::setQoSType(QoSType type)
+{
+	data->qosType = type;
+}
+
+SAPChannelInfo::QoSPriority SAPChannelInfo::qosPriority() const
+{
+	return data->qosPriority;
+}
+
+void SAPChannelInfo::setQoSPriority(QoSPriority priority)
+{
+	data->qosPriority = priority;
+}
+
+SAPChannelInfo::QoSDataRate SAPChannelInfo::qosDataRate() const
+{
+	return data->qosDataRate;
+}
+
+void SAPChannelInfo::setQoSDataRate(QoSDataRate rate)
+{
+	data->qosDataRate = rate;
+}
diff --git a/sapchannelinfo.h b/sapchannelinfo.h
new file mode 100644
index 0000000..e6db58b
--- /dev/null
+++ b/sapchannelinfo.h
@@ -0,0 +1,65 @@
+#ifndef SAPCHANNELINFO_H
+#define SAPCHANNELINFO_H
+
+#include <QtCore/QObject>
+#include <QtCore/QSharedDataPointer>
+
+class SAPChannelInfoData;
+
+class SAPChannelInfo
+{
+	Q_GADGET
+
+public:
+	SAPChannelInfo();
+	SAPChannelInfo(const SAPChannelInfo &);
+	SAPChannelInfo &operator=(const SAPChannelInfo &);
+	~SAPChannelInfo();
+
+	enum PayloadType {
+		PayloadNone = 0,
+		PayloadBinary = 1,
+		PayloadJson = 2,
+		PayloadAll = 0xFF
+	};
+
+	enum QoSType {
+		QoSUnrestrictedInOrder = 0,
+		QoSUnrestricted = 1,
+		QoSRestrictedInOrder = 2,
+		QoSRestricted = 3,
+		QoSReliabilityDisable = 4,
+		QoSReliabilityEnable = 5
+	};
+
+	enum QoSPriority {
+		QoSPriorityLow = 0,
+		QoSPriorityMedium,
+		QoSPriorityHigh
+	};
+
+	enum QoSDataRate {
+		QoSDataRateLow = 0,
+		QoSDataRateHigh
+	};
+
+	unsigned short channelId() const;
+	void setChannelId(unsigned short id);
+
+	PayloadType payloadType() const;
+	void setPayloadType(PayloadType type);
+
+	QoSType qosType() const;
+	void setQoSType(QoSType type);
+
+	QoSPriority qosPriority() const;
+	void setQoSPriority(QoSPriority priority);
+
+	QoSDataRate qosDataRate() const;
+	void setQoSDataRate(QoSDataRate rate);
+
+private:
+	QSharedDataPointer<SAPChannelInfoData> data;
+};
+
+#endif // SAPCHANNELINFO_H
diff --git a/sapconnection.cc b/sapconnection.cc
new file mode 100644
index 0000000..970ad11
--- /dev/null
+++ b/sapconnection.cc
@@ -0,0 +1,38 @@
+#include "sappeer.h"
+#include "sapconnection.h"
+
+SAPConnection::SAPConnection(SAPPeer *peer, const QString &profile)
+    : QObject(peer), _profile(profile)
+{
+}
+
+SAPPeer* SAPConnection::peer()
+{
+	return static_cast<SAPPeer*>(parent());
+}
+
+QString SAPConnection::profile() const
+{
+	return _profile;
+}
+
+SAPSocket * SAPConnection::getSocket(int channelId)
+{
+	return _sockets.value(channelId, 0);
+}
+
+QList<SAPSocket*> SAPConnection::sockets()
+{
+	return _sockets.values();
+}
+
+void SAPConnection::close()
+{
+	// TODO
+}
+
+void SAPConnection::setSocket(int channelId, SAPSocket *socket)
+{
+	Q_ASSERT(!_sockets.contains(channelId));
+	_sockets.insert(channelId, socket);
+}
diff --git a/sapconnection.h b/sapconnection.h
new file mode 100644
index 0000000..7702e43
--- /dev/null
+++ b/sapconnection.h
@@ -0,0 +1,43 @@
+#ifndef SAPCONNECTION_H
+#define SAPCONNECTION_H
+
+#include <QtCore/QObject>
+#include <QtCore/QMap>
+
+class SAPPeer;
+class SAPSocket;
+
+class SAPConnection : public QObject
+{
+	Q_OBJECT
+
+	SAPConnection(SAPPeer *peer, const QString &profile);
+
+public:
+	SAPPeer *peer();
+
+	QString profile() const;
+
+	SAPSocket *getSocket(int channelId);
+
+	QList<SAPSocket*> sockets();
+
+public slots:
+	void close();
+
+signals:
+	void connected();
+	void error(int errorCode);
+	void disconnected();
+
+private:
+	void setSocket(int channelId, SAPSocket *socket);
+
+private:
+	const QString _profile;
+	QMap<int, SAPSocket*> _sockets;
+
+	friend class SAPPeer;
+};
+
+#endif // SAPCONNECTION_H
diff --git a/sapconnectionrequest.cc b/sapconnectionrequest.cc
new file mode 100644
index 0000000..4842ff6
--- /dev/null
+++ b/sapconnectionrequest.cc
@@ -0,0 +1,40 @@
+#include "sappeer.h"
+#include "sapconnection.h"
+#include "sapconnectionrequest.h"
+
+SAPConnectionRequest::SAPConnectionRequest(SAPConnection *conn, int initiatorId, int acceptorId)
+    : _conn(conn), _initiator(initiatorId), _acceptor(acceptorId)
+{
+}
+
+SAPConnection *SAPConnectionRequest::connection()
+{
+	return _conn;
+}
+
+SAPPeer * SAPConnectionRequest::peer()
+{
+	return connection()->peer();
+}
+
+void SAPConnectionRequest::accept()
+{
+	peer()->acceptServiceConnection(this, 0);
+	delete this;
+}
+
+void SAPConnectionRequest::reject(int reason)
+{
+	peer()->acceptServiceConnection(this, reason);
+	delete this;
+}
+
+int SAPConnectionRequest::initiatorId() const
+{
+	return _initiator;
+}
+
+int SAPConnectionRequest::acceptorId() const
+{
+	return _acceptor;
+}
diff --git a/sapconnectionrequest.h b/sapconnectionrequest.h
new file mode 100644
index 0000000..bbb4a0e
--- /dev/null
+++ b/sapconnectionrequest.h
@@ -0,0 +1,32 @@
+#ifndef SAPCONNECTIONREQUEST_H
+#define SAPCONNECTIONREQUEST_H
+
+#include <QtCore/QList>
+
+class SAPPeer;
+class SAPConnection;
+
+class SAPConnectionRequest
+{
+	SAPConnectionRequest(SAPConnection *conn, int initiatorId, int acceptorId);
+
+public:
+	SAPPeer * peer();
+	SAPConnection *connection();
+
+	void accept();
+	void reject(int reason = 1);
+
+protected:
+	int initiatorId() const;
+	int acceptorId() const;
+
+private:
+	SAPConnection *_conn;
+	int _initiator;
+	int _acceptor;
+
+	friend class SAPPeer;
+};
+
+#endif // SAPCONNECTIONREQUEST_H
diff --git a/sapd.pro b/sapd.pro
new file mode 100644
index 0000000..7b5a154
--- /dev/null
+++ b/sapd.pro
@@ -0,0 +1,73 @@
+TARGET = sapd
+TEMPLATE = app
+QT       += core dbus
+QT       -= gui
+CONFIG   += console
+
+greaterThan(QT_MAJOR_VERSION, 4) {
+    QT       += bluetooth
+} else {
+    CONFIG   += mobility
+    MOBILITY += connectivity
+    DEFINES  += DESKTOP
+    SOURCES  += hfpag.cc
+    HEADERS  += hfpag.h
+}
+
+CONFIG    += link_pkgconfig
+PKGCONFIG += bluez openssl
+
+SOURCES += main.cc \
+    sapbtlistener.cc \
+    wmspeer.cc \
+    sapbtpeer.cc \
+    saprotocol.cc \
+    keys/Tout.c \
+    keys/Tin.c \
+    keys/T2.c \
+    keys/T1.c \
+    keys/T0.c \
+    keys/psk_table.c \
+    keys/finalT1.c \
+    keys/finalT0.c \
+    keys/DecTout.c \
+    keys/DecTin.c \
+    keys/DecT2.c \
+    keys/DecT1.c \
+    keys/DecT0.c \
+    wmscrypt.cc \
+    crc16.cc \
+    sapsocket.cc \
+    sappeer.cc \
+    capabilityagent.cc \
+    qtjson/json.cc \
+    sapmanager.cc \
+    sapserviceinfo.cc \
+    sapchannelinfo.cc \
+    sapconnection.cc \
+    sapconnectionrequest.cc \
+    capabilitypeer.cc \
+    hostmanageragent.cc \
+    hostmanagerconn.cc
+
+HEADERS += \
+    sapbtlistener.h \
+    wmspeer.h \
+    sapbtpeer.h \
+    saprotocol.h \
+    wmskeys.h \
+    wmscrypt.h \
+    crc16.h \
+    sapsocket.h \
+    sapagent.h \
+    sappeer.h \
+    capabilityagent.h \
+    qtjson/json.h \
+    sapmanager.h \
+    sapserviceinfo.h \
+    sapchannelinfo.h \
+    sapconnection.h \
+    sapconnectionrequest.h \
+    capabilitypeer.h \
+    hostmanageragent.h \
+    hostmanagerconn.h
diff --git a/sapmanager.cc b/sapmanager.cc
new file mode 100644
index 0000000..c9d7df6
--- /dev/null
+++ b/sapmanager.cc
@@ -0,0 +1,125 @@
+#include <QtCore/QDebug>
+#include "saprotocol.h"
+#include "sapmanager.h"
+
+static SAPManager *manager = 0;
+
+SAPManager::SAPManager(QObject *parent) :
+    QObject(parent)
+{
+}
+
+SAPManager * SAPManager::instance()
+{
+	if (!manager) {
+		manager = new SAPManager;
+	}
+	return manager;
+}
+
+int SAPManager::registerServiceAgent(const SAPServiceInfo &service, SAPAgent *agent)
+{
+	const QString profile = service.profile();
+	const SAPServiceInfo::Role role = service.role();
+
+	QHash<QString, int> *profiles = profilesByRole(role);
+
+	if (!profiles) return -1;
+	if (profile.isEmpty()) return -1;
+	if (profiles->contains(profile)) return -1;
+
+	int agentId = findUnusedAgentId();
+	if (agentId < 0) return -1;
+
+	RegisteredAgent ragent;
+	ragent.agentId = agentId;
+	ragent.agent = agent;
+	ragent.info = service;
+
+	_agents.insert(agentId, ragent);
+
+	profiles->insert(profile, agentId);
+
+	return agentId;
+}
+
+void SAPManager::unregisterServiceAgent(int agentId)
+{
+	if (_agents.contains(agentId)) {
+		const RegisteredAgent &ragent = _agents[agentId];
+		const QString profile = ragent.info.profile();
+		const SAPServiceInfo::Role role = ragent.info.role();
+
+		QHash<QString, int> *profiles = profilesByRole(role);
+		Q_ASSERT(profiles);
+
+		profiles->remove(profile);
+		_agents.remove(agentId);
+	}
+}
+
+void SAPManager::unregisterServiceAgent(const QString &profile, SAPServiceInfo::Role role)
+{
+	int agentId = registeredAgentId(profile, role);
+
+	if (agentId >= 0) {
+		unregisterServiceAgent(agentId);
+	}
+}
+
+int SAPManager::registeredAgentId(const QString &profile, SAPServiceInfo::Role role)
+{
+	QHash<QString, int> *profiles = profilesByRole(role);
+	if (!profiles) return -1;
+
+	return profiles->value(profile, -1);
+}
+
+bool SAPManager::isRegisteredAgent(int agentId) const
+{
+	return _agents.contains(agentId);
+}
+
+SAPAgent * SAPManager::agent(int agentId)
+{
+	if (!_agents.contains(agentId)) return 0;
+	return _agents.value(agentId).agent;
+}
+
+SAPServiceInfo SAPManager::serviceInfo(int agentId) const
+{
+	return _agents.value(agentId).info;
+}
+
+QSet<QString> SAPManager::allProfiles()
+{
+	return QSet<QString>::fromList(_consumerProfiles.keys())
+	        + QSet<QString>::fromList(_providerProfiles.keys());
+}
+
+int SAPManager::findUnusedAgentId() const
+{
+	if (_agents.size() > 20000) {
+		qWarning() << "Ran out of agent ids!";
+		return -1;
+	}
+
+	int id = 1;
+	while (_agents.contains(id)) {
+		id++;
+	}
+
+	return id;
+}
+
+QHash<QString, int>* SAPManager::profilesByRole(SAPServiceInfo::Role role)
+{
+	switch (role) {
+	case SAPServiceInfo::RoleProvider:
+		return &_providerProfiles;
+	case SAPServiceInfo::RoleConsumer:
+		return &_consumerProfiles;
+	default:
+		return 0;
+	}
+}
diff --git a/sapmanager.h b/sapmanager.h
new file mode 100644
index 0000000..e9f60cc
--- /dev/null
+++ b/sapmanager.h
@@ -0,0 +1,54 @@
+#ifndef SAPMANAGER_H
+#define SAPMANAGER_H
+
+#include <QtCore/QObject>
+#include <QtCore/QMap>
+#include <QtCore/QHash>
+#include "sapserviceinfo.h"
+
+class SAPAgent;
+
+class SAPManager : public QObject
+{
+	Q_OBJECT
+
+	explicit SAPManager(QObject *parent = 0);
+	Q_DISABLE_COPY(SAPManager)
+
+public:
+	static SAPManager * instance();
+
+	int registerServiceAgent(const SAPServiceInfo &service, SAPAgent *agent);
+	void unregisterServiceAgent(int agentId);
+	void unregisterServiceAgent(const QString &profile, SAPServiceInfo::Role role);
+
+	int registeredAgentId(const QString &profile, SAPServiceInfo::Role role);
+
+	bool isRegisteredAgent(int agentId) const;
+	SAPAgent *agent(int agentId);
+	SAPServiceInfo serviceInfo(int agentId) const;
+
+	QSet<QString> allProfiles();
+
+signals:
+
+public slots:
+
+private:
+	int findUnusedAgentId() const;
+	QHash<QString, int>* profilesByRole(SAPServiceInfo::Role role);
+
+private:
+	struct RegisteredAgent {
+		int agentId;
+		SAPServiceInfo info;
+		SAPAgent *agent;
+	};
+
+	QMap<int, RegisteredAgent> _agents;
+
+	QHash<QString, int> _consumerProfiles;
+	QHash<QString, int> _providerProfiles;
+};
+
+#endif // SAPMANAGER_H
diff --git a/sappeer.cc b/sappeer.cc
new file mode 100644
index 0000000..af843e4
--- /dev/null
+++ b/sappeer.cc
@@ -0,0 +1,379 @@
+#include <QtCore/QDebug>
+
+#include "sapmanager.h"
+#include "sapconnection.h"
+#include "sapconnectionrequest.h"
+#include "sapsocket.h"
+#include "sapchannelinfo.h"
+#include "capabilityagent.h"
+#include "capabilitypeer.h"
+#include "wmspeer.h"
+#include "sappeer.h"
+
+SAPPeer::SAPPeer(SAProtocol::Role role, const QString &localName, const QString &peerName, QObject *parent) :
+    QObject(parent),
+    _wms(new WMSPeer(role, localName, peerName, this)),
+    _role(role),
+    _localName(localName), _peerName(peerName)
+{
+}
+
+SAPConnection * SAPPeer::createServiceConnection(const QString &profile, const QString &requesterProfile, SAPServiceInfo::Role requesterRole)
+{
+	SAPManager *manager = SAPManager::instance();
+
+	int initiator = manager->registeredAgentId(requesterProfile, requesterRole);
+
+	if (initiator < 0) {
+		qWarning() << "Requester profile not found:" << requesterProfile;
+		return 0;
+	}
+
+	// Search remote agent id in capability database.
+	CapabilityPeer *capPeer = capabilityPeer();
+
+	int acceptor = capPeer->remoteAgentId(profile, SAPServiceInfo::oppositeRole(requesterRole));
+	if (acceptor < 0) {
+		qWarning() << "Remote profile not found:" << profile;
+		return 0;
+	}
+
+	SAPServiceInfo sInfo = manager->serviceInfo(initiator);
+	SAPConnection *conn = new SAPConnection(this, profile);
+
+	SAProtocol::ServiceConnectionRequestFrame request;
+	request.messageType = SAProtocol::ServiceConnectionRequest;
+	request.initiatorId = initiator;
+	request.acceptorId = acceptor;
+	request.profile = profile;
+
+	if (request.acceptorId == SAProtocol::capabilityDiscoveryAgentId) {
+		// Addressing the capability discovery service? Be anonymous about it
+		request.initiatorId = SAProtocol::capabilityDiscoveryAgentId;
+	}
+
+	foreach (const SAPChannelInfo &cInfo, sInfo.channels()) {
+		SAProtocol::ServiceConnectionRequestSession session;
+
+		int sessionId = findUnusedSessionId();
+
+		session.sessionId = sessionId;
+		session.channelId = cInfo.channelId();
+		session.qosType = cInfo.qosType();
+		session.qosDataRate = cInfo.qosDataRate();
+		session.qosPriority = cInfo.qosPriority();
+		session.payloadType = cInfo.payloadType();
+
+		request.sessions.append(session);
+
+		SAPSocket *socket = new SAPSocket(conn, sessionId);
+		// TODO set socket QoS parameters
+		conn->setSocket(session.channelId, socket);
+
+		_sessions.insert(sessionId, socket);
+	}
+
+	writeToSession(SAProtocol::defaultSessionId,
+	               SAProtocol::packServiceConnectionRequestFrame(request));
+
+	return conn;
+}
+
+SAProtocol::Role SAPPeer::role() const
+{
+	return _role;
+}
+
+QString SAPPeer::localName() const
+{
+	return _localName;
+}
+
+QString SAPPeer::peerName() const
+{
+	return _peerName;
+}
+
+CapabilityPeer *SAPPeer::capabilityPeer()
+{
+	return findChild<CapabilityPeer*>();
+}
+
+bool SAPPeer::writeToSession(int session, const QByteArray &data)
+{
+	if (session == SAProtocol::defaultSessionId) {
+		// Session is default (always open) or already open
+		sendFrame(SAProtocol::packFrame(session, data));
+		return true;
+	} else {
+		SAPSocket *socket = _sessions.value(session, 0);
+		if (socket && socket->isOpen()) {
+			sendFrame(SAProtocol::packFrame(session, data));
+			return true;
+		} else {
+			qWarning() << "Session" << session << "not open yet";
+			return false;
+		}
+	}
+}
+
+void SAPPeer::acceptServiceConnection(SAPConnectionRequest *connReq, int statusCode)
+{
+	SAPConnection *conn = connReq->connection();
+
+	SAProtocol::ServiceConnectionResponseFrame resp;
+	resp.messageType = SAProtocol::ServiceConnectionResponse;
+	resp.acceptorId = connReq->acceptorId();
+	resp.initiatorId = connReq->initiatorId();
+	resp.profile = conn->profile();
+	resp.statusCode = statusCode;
+
+	foreach (const SAPSocket *socket, conn->sockets()) {
+		resp.sessions.push_back(socket->sessionId());
+	}
+
+	qDebug() << "acceptServiceConection" << statusCode;
+
+	writeToSession(SAProtocol::defaultSessionId,
+	               SAProtocol::packServiceConnectionResponseFrame(resp));
+
+	if (statusCode == 0) {
+		foreach (SAPSocket *socket, conn->sockets()) {
+			socket->setOpen(true);
+			emit socket->connected();
+		}
+
+		emit conn->connected();
+	} else {
+		// Cancel any partial opened sessions
+		foreach (SAPSocket *socket, conn->sockets()) {
+			_sessions.remove(socket->sessionId());
+			delete socket;
+		}
+		delete conn; // Also deletes child sockets
+	}
+
+	// Can't delete connReq now because we must return to it.
+	// It will take care of itself.
+}
+
+void SAPPeer::handleSessionData(int session, const QByteArray &data)
+{
+	if (session == SAProtocol::defaultSessionId) {
+		// Default session data frame.
+		handleDefaultSessionMessage(data);
+		return;
+	} else {
+		SAPSocket *socket = _sessions.value(session, 0);
+		if (socket && socket->isOpen()) {
+			socket->acceptIncomingData(data);
+		} else {
+			qWarning() << "Got information for a session that's not yet open!" << session;
+		}
+	}
+}
+
+void SAPPeer::handleConnected()
+{
+	emit connected();
+
+	// Manually call the capability agent in order to trigger initial capability discovery.
+	CapabilityAgent *caps = CapabilityAgent::instance();
+	caps->peerFound(this);
+}
+
+void SAPPeer::handleDisconnected()
+{
+	// TODO
+	emit disconnected();
+	deleteLater();
+}
+
+int SAPPeer::findUnusedSessionId() const
+{
+	if (_sessions.size() > SAProtocol::maxSessionId) {
+		qWarning() << "Ran out of session ids!";
+		return -1;
+	}
+
+	int id = 1;
+	while (_sessions.contains(id)) {
+		id++;
+	}
+
+	Q_ASSERT(id <= SAProtocol::maxSessionId && id != SAProtocol::defaultSessionId);
+
+	return id;
+}
+
+void SAPPeer::handleDefaultSessionMessage(const QByteArray &message)
+{
+	const quint8 message_type = message[0];
+
+	switch (message_type) {
+	case SAProtocol::ServiceConnectionRequest: {
+		SAPManager *manager = SAPManager::instance();
+		SAProtocol::ServiceConnectionRequestFrame req = SAProtocol::unpackServiceConnectionRequestFrame(message);
+		bool ok = true;
+
+		qDebug() << "Service connection request to profile" << req.profile;
+
+		SAPAgent *agent = manager->agent(req.acceptorId);
+
+		if (!agent) {
+			qWarning() << "Requested agent does not exist";
+			ok = false;
+		}
+
+		foreach (const SAProtocol::ServiceConnectionRequestSession &s, req.sessions) {
+			if (_sessions.contains(s.sessionId)) {
+				qWarning() << "Requested session is already in use";
+				ok = false;
+			}
+		}
+
+		if (!ok) {
+			SAProtocol::ServiceConnectionResponseFrame resp;
+			resp.messageType = SAProtocol::ServiceConnectionResponse;
+			resp.acceptorId = req.acceptorId;
+			resp.initiatorId = req.initiatorId;
+			resp.profile = req.profile;
+			resp.statusCode = 1;
+
+			foreach (const SAProtocol::ServiceConnectionRequestSession &s, req.sessions) {
+				resp.sessions.push_back(s.sessionId);
+			}
+
+			writeToSession(SAProtocol::defaultSessionId,
+			               SAProtocol::packServiceConnectionResponseFrame(resp));
+			return;
+		}
+
+		SAPConnection *conn = new SAPConnection(this, req.profile);
+
+		foreach (const SAProtocol::ServiceConnectionRequestSession &s, req.sessions) {
+			SAPSocket *socket = new SAPSocket(conn, s.sessionId);
+			// TODO set socket QoS parameters
+			conn->setSocket(s.channelId, socket);
+
+			_sessions.insert(s.sessionId, socket);
+		}
+
+		SAPConnectionRequest *connReq = new SAPConnectionRequest(conn, req.initiatorId, req.acceptorId);
+		agent->requestConnection(connReq);
+
+#if 0
+		resp.messageType = SAProtocol::ServiceConnectionResponse;
+		resp.acceptorId = req.acceptorId;
+		resp.initiatorId = req.initiatorId;
+		resp.profile = req.profile;
+		resp.statusCode = 0;
+
+		QList<SAPSocket*> created_sockets;
+
+		SAPAgent *agent = _profiles.value(req.profile, 0);
+		if (agent) {
+			foreach (const SAProtocol::ServiceConnectionRequestSession &s, req.sessions) {
+				if (_openSessions.contains(s.sessionId)) {
+					// Session ID already used.
+					resp.statusCode = 1;
+					break;
+				}
+
+				SAPSocket *socket = new SAPSocket(this, s.sessionId, this);
+
+				qDebug() << "Requesting connection to" << req.profile << s.channelId;
+
+				if (!agent->acceptConnection(req.profile, s.channelId, socket)) {
+					resp.statusCode = 2;
+					break;
+				}
+
+				// Save for later. We can't emit the connected signal right now
+				// because data might be written to the socket even before
+				// we send the response.
+				created_sockets.append(socket);
+
+				resp.sessions.append(s.sessionId);
+			}
+
+			if (resp.statusCode != 0) {
+				resp.sessions.clear(); // TODO Is this correct? Who knows!
+			}
+		}
+
+		qDebug() << "Service connection request statusCode=" << resp.statusCode;
+
+		writeToSession(SAProtocol::defaultSessionId,
+		               SAProtocol::packServiceConnectionResponseFrame(resp));
+
+		if (resp.statusCode == 0) {
+			foreach (SAPSocket *socket, created_sockets) {
+				const int session = socket->sessionId();
+				_openSessions.insert(session);
+				qDebug() << "Session" << session << "now live";
+
+				emit socket->connected();
+			}
+		} else {
+			foreach (SAPSocket *socket, created_sockets) {
+				emit socket->disconnected();
+				socket->deleteLater();
+			}
+		}
+#endif
+
+		break;
+	}
+	case SAProtocol::ServiceConnectionResponse: {
+		SAProtocol::ServiceConnectionResponseFrame frame = SAProtocol::unpackServiceConnectionResponseFrame(message);
+
+		bool ok = frame.statusCode == 0;
+
+		if (!ok) {
+			qWarning() << "Failure to create a service connection";
+		} else {
+			qDebug() << "Service connection OK";
+		}
+
+		if (frame.sessions.isEmpty()) {
+			qWarning() << "No sessions in frame, nothing to do";
+			return;
+		}
+
+		SAPSocket *firstSocket = _sessions.value(frame.sessions.first(), 0);
+		if (!firstSocket) {
+			qWarning() << "Unknown session id:" << frame.sessions.first();
+			return;
+		}
+
+		SAPConnection *conn = firstSocket->connection();
+
+		foreach (int session, frame.sessions) {
+			SAPSocket *socket = _sessions.value(session, 0);
+			if (socket) {
+				if (socket->isOpen()) {
+					qWarning() << "Session" << session << "was already open?";
+				}
+
+				qDebug() << "Session" << session << "now live";
+				socket->setOpen(true);
+				emit socket->connected();
+			} else {
+				qWarning() << "Unknown session id:" << session;
+			}
+		}
+
+		if (ok) {
+			emit conn->connected();
+		} else {
+			emit conn->error(frame.statusCode);
+			emit conn->disconnected();
+		}
+
+		break;
+	}
+	default:
+		qWarning() << "Unknown default session message type:" << message_type;
+	}
+}
diff --git a/sappeer.h b/sappeer.h
new file mode 100644
index 0000000..5e565db
--- /dev/null
+++ b/sappeer.h
@@ -0,0 +1,69 @@
+#ifndef SAPPEER_H
+#define SAPPEER_H
+
+#include <QtCore/QObject>
+#include <QtCore/QQueue>
+
+#include "saprotocol.h"
+#include "sapserviceinfo.h"
+
+class WMSPeer;
+class CapabilityPeer;
+class SAPConnection;
+class SAPConnectionRequest;
+class SAPSocket;
+class SAPAgent;
+
+class SAPPeer : public QObject
+{
+	Q_OBJECT
+public:
+	explicit SAPPeer(SAProtocol::Role role, const QString &localName, const QString &peerName, QObject *parent = 0);
+
+	SAPConnection* createServiceConnection(const QString &profile, const QString &requesterProfile, SAPServiceInfo::Role requesterRole);
+
+	SAProtocol::Role role() const;
+	QString localName() const;
+	QString peerName() const;
+
+signals:
+	void connected();
+	void disconnected();
+
+protected:
+	/** Transmits a frame to the underlying transport. */
+	virtual void sendFrame(const QByteArray &data) = 0;
+
+	CapabilityPeer *capabilityPeer();
+
+	/** Writes data to the remote. */
+	bool writeToSession(int session, const QByteArray &data);
+
+	void acceptServiceConnection(SAPConnectionRequest *connReq, int statusCode);
+
+	/** Distributes data to the appropiate socket. */
+	void handleSessionData(int session, const QByteArray &data);
+
+	/** Perform service discovery once connected. */
+	void handleConnected();
+	void handleDisconnected();
+
+private:
+	int findUnusedSessionId() const;
+	void handleDefaultSessionMessage(const QByteArray &message);
+
+protected:
+	WMSPeer *_wms;
+
+private:
+	const SAProtocol::Role _role;
+	const QString _localName;
+	const QString _peerName;
+
+	QMap<int, SAPSocket*> _sessions;
+
+	friend class SAPSocket;
+	friend class SAPConnectionRequest;
+};
+
+#endif // SAPPEER_H
diff --git a/saprotocol.cc b/saprotocol.cc
new file mode 100644
index 0000000..5a21762
--- /dev/null
+++ b/saprotocol.cc
@@ -0,0 +1,499 @@
+#include <QtCore/QDebug>
+#include <QtEndian>
+#include "saprotocol.h"
+
+const QBluetoothUuid SAProtocol::dataServiceUuid(QLatin1String("a49eb41e-cb06-495c-9f4f-aa80a90cdf4a"));
+const QBluetoothUuid SAProtocol::nudgeServiceUuid(QLatin1String("a49eb41e-cb06-495c-9f4f-bb80a90cdf00"));
+
+const QLatin1String SAProtocol::capabilityDiscoveryProfile("/System/Reserved/ServiceCapabilityDiscovery");
+
+namespace
+{
+
+template<typename T>
+inline T read(const QByteArray &data, int &offset)
+{
+	T unswapped;
+	qMemCopy(&unswapped, &data.constData()[offset], sizeof(T)); // Unaligned access warning!
+	offset += sizeof(T);
+	return qFromBigEndian<T>(unswapped);
+}
+
+template<typename T>
+inline void append(QByteArray &data, const T &value)
+{
+	T swapped = qToBigEndian<T>(value);
+	data.append(reinterpret_cast<const char*>(&swapped), sizeof(T));
+}
+
+}
+
+SAProtocol::SAProtocol()
+{
+}
+
+SAProtocol::PeerDescription SAProtocol::unpackPeerDescription(const QByteArray &data)
+{
+	PeerDescription desc;
+	if (data.size() < 24) {
+		qWarning() << "Peer description too small";
+		desc.messageType = 0;
+		return desc;
+	}
+
+	int offset = 0;
+
+	desc.messageType = read<quint8>(data, offset);
+
+	if (desc.messageType != 5 && desc.messageType != 6) {
+		qWarning() << "Unknown message type:" << desc.messageType;
+		return desc;
+	}
+
+	desc.accessoryProtocolVersion = read<quint16>(data, offset);
+	desc.accessorySoftwareVersion = read<quint16>(data, offset);
+
+	if (desc.messageType == 6) {
+		desc.status = read<quint8>(data, offset);
+		desc.errorCode = read<quint8>(data, offset);
+	} else {
+		// God knows WHYYY.
+		desc.status = read<quint8>(data, offset);
+	}
+	desc.APDUSize = read<quint32>(data, offset);
+	desc.SSDUSize = read<quint16>(data, offset);
+	desc.sessions = read<quint16>(data, offset);
+	desc.timeout = read<quint16>(data, offset);
+	desc.unk_1 = read<quint8>(data, offset);
+	desc.unk_2 = read<quint16>(data, offset);
+	desc.unk_3 = read<quint8>(data, offset);
+
+	int marker = data.indexOf(peerDescriptionSeparator, offset);
+	if (marker == -1) {
+		qWarning() << "No product in peer description";
+		return desc;
+	}
+	desc.product = QString::fromUtf8(&data.constData()[offset], marker - offset);
+	offset = marker + 1;
+
+	marker = data.indexOf(peerDescriptionSeparator, offset);
+	if (marker == -1) {
+		qWarning() << "No manufacturer in peer description";
+		return desc;
+	}
+	desc.manufacturer = QString::fromUtf8(&data.constData()[offset], marker - offset);
+	offset = marker + 1;
+
+	marker = data.indexOf(peerDescriptionSeparator, offset);
+	if (marker == -1) {
+		qWarning() << "No name in peer description";
+		return desc;
+	}
+	desc.name = QString::fromUtf8(&data.constData()[offset], marker - offset);
+	offset = marker + 1;
+
+	marker = data.indexOf(peerDescriptionSeparator, offset);
+	if (marker == -1) {
+		qWarning() << "No profile in peer description";
+		return desc;
+	}
+	desc.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
+	offset = marker + 1;
+
+	return desc;
+}
+
+QByteArray SAProtocol::packPeerDescription(const PeerDescription &desc)
+{
+	QByteArray data;
+
+	switch (desc.messageType) {
+	case 5:
+		data.reserve(20 + 4 + desc.product.length() + desc.manufacturer.length() + desc.name.length() + desc.profile.length());
+		break;
+	case 6:
+		data.reserve(21 + 4 + desc.product.length() + desc.manufacturer.length() + desc.name.length() + desc.profile.length());
+		break;
+	default:
+		qWarning() << "Unknown message type:" << desc.messageType;
+		return data;
+	}
+
+	append<quint8>(data, desc.messageType);
+
+	append<quint16>(data, desc.accessoryProtocolVersion);
+	append<quint16>(data, desc.accessorySoftwareVersion);
+
+	if (desc.messageType == 6) {
+		append<quint8>(data, desc.status);
+		append<quint8>(data, desc.errorCode);
+	} else {
+		append<quint8>(data, desc.status);
+	}
+
+	append<quint32>(data, desc.APDUSize);
+	append<quint16>(data, desc.SSDUSize);
+	append<quint16>(data, desc.sessions);
+	append<quint16>(data, desc.timeout);
+	append<quint8>(data, desc.unk_1);
+	append<quint16>(data, desc.unk_2);
+	append<quint8>(data, desc.unk_3);
+
+	data.append(desc.product.toUtf8());
+	data.append(peerDescriptionSeparator);
+
+	data.append(desc.manufacturer.toUtf8());
+	data.append(peerDescriptionSeparator);
+
+	data.append(desc.name.toUtf8());
+	data.append(peerDescriptionSeparator);
+
+	data.append(desc.profile.toUtf8());
+	data.append(peerDescriptionSeparator);
+
+	return data;
+}
+
+SAProtocol::SecurityFrame SAProtocol::unpackSecurityFrame(const QByteArray &data)
+{
+	SecurityFrame sframe;
+
+	int offset = 0;
+	sframe.type = static_cast<SecurityFrameType>(read<quint8>(data, offset));
+	sframe.authType = read<quint8>(data, offset);
+
+	if (sframe.authType != 0) {
+		qWarning() << "TODO Unhandled auth type frame";
+		// Those frames may not contain version, data fields from below.
+		sframe.version = 0;
+		sframe.dataType = 0;
+		return sframe;
+	}
+
+	sframe.version = read<quint8>(data, offset);
+	sframe.dataType = read<quint8>(data, offset);
+	int len = read<quint8>(data, offset) - 3;
+	sframe.data = data.mid(offset, len);
+
+	if (offset + len < data.size()) {
+		qWarning() << "Ignoring trailing data on security frame";
+	} else if (offset + len > data.size()) {
+		qWarning() << "Security frame too short";
+	}
+
+	return sframe;
+}
+
+QByteArray SAProtocol::packSecurityFrame(const SAProtocol::SecurityFrame &sframe)
+{
+	QByteArray data;
+
+	append<quint8>(data, sframe.type);
+	append<quint8>(data, sframe.authType);
+
+	switch (sframe.authType) {
+	case 0:
+		append<quint8>(data, sframe.version);
+		append<quint8>(data, sframe.dataType);
+		append<quint8>(data, sframe.data.length() + 3); // Includes header size
+
+		data.append(sframe.data);
+		break;
+	default:
+		qWarning() << "TODO Unhandled auth type frame";
+		break;
+	}
+
+	return data;
+}
+
+SAProtocol::Frame SAProtocol::unpackFrame(const QByteArray &data)
+{
+	Q_ASSERT(data.size() > 2);
+
+	Frame frame;
+
+	frame.protocolVersion = (data[0] & 0xE0) >> 5;
+	if (frame.protocolVersion != currentProtocolVersion) {
+		qWarning() << "Unknown protocol version: " << frame.protocolVersion;
+		return frame;
+	}
+
+	frame.type = static_cast<FrameType>((data[0] & 0x10) >> 4);
+	frame.sessionId = ((data[0] & 0xF) << 6) | ((data[1] & 0xFC) >> 2);
+
+	frame.data = data.mid(2);
+
+	return frame;
+}
+
+QByteArray SAProtocol::packFrame(const Frame &frame)
+{
+	char header[2];
+
+	header[0] = (frame.protocolVersion << 5) & 0xE0;
+	header[0] |= (frame.type << 4) & 0x10;
+	header[0] |= ((frame.sessionId & 0x3C0) >> 6) & 0xF;
+	header[1] = ((frame.sessionId & 0x3F) << 2) & 0xFC;
+
+	QByteArray data = frame.data;
+	return data.prepend(reinterpret_cast<char*>(&header), 2);
+}
+
+QByteArray SAProtocol::packFrame(quint16 sessionId, const QByteArray &data)
+{
+	Frame frame;
+	frame.protocolVersion = currentProtocolVersion;
+	frame.type = FrameData;
+	frame.sessionId = sessionId;
+	frame.data = data;
+	return packFrame(frame);
+}
+
+SAProtocol::ServiceConnectionRequestFrame SAProtocol::unpackServiceConnectionRequestFrame(const QByteArray &data)
+{
+	ServiceConnectionRequestFrame frame;
+	int offset = 0;
+
+	frame.messageType = read<quint8>(data, offset);
+	frame.acceptorId = read<quint16>(data, offset);
+	frame.initiatorId = read<quint16>(data, offset);
+
+	int marker = data.indexOf(';', offset);
+	if (marker == -1) {
+		qWarning() << "No profile in conn request";
+		return frame;
+	}
+
+	frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
+	offset = marker + 1;
+
+	int num_sessions = read<quint16>(data, offset);
+
+	while (num_sessions > 0) {
+		ServiceConnectionRequestSession session;
+		session.sessionId = read<quint16>(data, offset);
+		session.channelId = read<quint16>(data, offset);
+		session.qosType = read<quint8>(data, offset);
+		session.qosDataRate = read<quint8>(data, offset);
+		session.qosPriority = read<quint8>(data, offset);
+		session.payloadType = read<quint8>(data, offset);
+
+		frame.sessions.append(session);
+
+		num_sessions--;
+	}
+
+	return frame;
+}
+
+QByteArray SAProtocol::packServiceConnectionRequestFrame(const ServiceConnectionRequestFrame &frame)
+{
+	QByteArray data;
+
+	append<quint8>(data, frame.messageType);
+	append<quint16>(data, frame.acceptorId);
+	append<quint16>(data, frame.initiatorId);
+
+	data.append(frame.profile.toUtf8());
+	data.append(';'); // Some kind of terminator
+
+	append<quint16>(data, frame.sessions.size());
+
+	foreach (const ServiceConnectionRequestSession &session, frame.sessions) {
+		append<quint16>(data, session.sessionId);
+		append<quint16>(data, session.channelId);
+		append<quint8>(data, session.qosType);
+		append<quint8>(data, session.qosDataRate);
+		append<quint8>(data, session.qosPriority);
+		append<quint8>(data, session.payloadType);
+	}
+
+	return data;
+}
+
+SAProtocol::ServiceConnectionResponseFrame SAProtocol::unpackServiceConnectionResponseFrame(const QByteArray &data)
+{
+	ServiceConnectionResponseFrame frame;
+	int offset = 0;
+
+	frame.messageType = read<quint8>(data, offset);
+	frame.acceptorId = read<quint8>(data, offset);
+	frame.initiatorId = read<quint8>(data, offset);
+
+	int marker = data.indexOf(';', offset);
+	if (marker == -1) {
+		qWarning() << "No profile in conn response";
+		return frame;
+	}
+
+	frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
+	offset = marker + 1;
+
+	frame.statusCode = read<quint8>(data, offset);
+
+	int num_sessions = read<quint16>(data, offset);
+
+	while (num_sessions > 0) {
+		int session = read<quint16>(data, offset);
+		frame.sessions.append(session);
+
+		num_sessions--;
+	}
+
+	return frame;
+}
+
+QByteArray SAProtocol::packServiceConnectionResponseFrame(const ServiceConnectionResponseFrame &frame)
+{
+	QByteArray data;
+
+	append<quint8>(data, frame.messageType);
+	append<quint16>(data, frame.acceptorId);
+	append<quint16>(data, frame.initiatorId);
+
+	data.append(frame.profile.toUtf8());
+	data.append(';');
+
+	append<quint8>(data, frame.statusCode);
+
+	append<quint16>(data, frame.sessions.size());
+
+	foreach (quint16 session, frame.sessions) {
+		append<quint16>(data, session);
+	}
+
+	return data;
+}
+
+SAProtocol::CapabilityDiscoveryQuery SAProtocol::unpackCapabilityDiscoveryQuery(const QByteArray &data)
+{
+	CapabilityDiscoveryQuery msg;
+	int offset = 0;
+
+	msg.messageType = static_cast<CapabilityDiscoveryMessageType>(read<quint8>(data, offset));
+	msg.queryType = read<quint8>(data, offset);
+
+	msg.checksum = read<quint32>(data, offset);
+
+	int num_records = read<quint8>(data, offset);
+
+	while (num_records > 0) {
+		int marker = data.indexOf(';', offset);
+		if (marker == -1) {
+			qWarning() << "Failure to parse all records!";
+			return msg;
+		}
+
+		msg.records.append(QString::fromUtf8(&data.constData()[offset], marker - offset));
+		offset = marker + 1;
+
+		num_records--;
+	}
+
+	return msg;
+}
+
+QByteArray SAProtocol::packCapabilityDiscoveryQuery(const CapabilityDiscoveryQuery &msg)
+{
+	QByteArray data;
+
+	append<quint8>(data, msg.messageType);
+	append<quint8>(data, msg.queryType);
+	append<quint32>(data, msg.checksum);
+
+	append<quint8>(data, msg.records.size());
+
+	foreach (const QString &record, msg.records) {
+		data.append(record.toUtf8());
+		data.append(';');
+	}
+
+	return data;
+}
+
+SAProtocol::CapabilityDiscoveryResponse SAProtocol::unpackCapabilityDiscoveryResponse(const QByteArray &data)
+{
+	CapabilityDiscoveryResponse msg;
+	int offset = 0;
+
+	msg.messageType = static_cast<CapabilityDiscoveryMessageType>(read<quint8>(data, offset));
+	msg.queryType = read<quint8>(data, offset);
+
+	msg.checksum = read<quint32>(data, offset);
+
+	int num_records = read<quint16>(data, offset);
+
+	while (num_records > 0) {
+		CapabilityDiscoveryProvider provider;
+
+		provider.uuid = read<quint16>(data, offset);
+
+		int marker = data.indexOf(';', offset);
+		if (marker == -1) {
+			qWarning() << "Failure to parse all providers!";
+			return msg;
+		}
+
+		provider.name = QString::fromUtf8(&data.constData()[offset], marker - offset);
+		offset = marker + 1;
+
+		int num_services = read<quint16>(data, offset);
+
+		while (num_services > 0) {
+			CapabilityDiscoveryService service;
+
+			service.componentId = read<quint16>(data, offset);
+
+			marker = data.indexOf(';', offset);
+			if (marker == -1) {
+				qWarning() << "Failure to parse all services!";
+				return msg;
+			}
+
+			service.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
+			offset = marker + 1;
+
+			service.aspVersion = read<quint16>(data, offset);
+			service.role = read<quint8>(data, offset);
+			service.connTimeout = read<quint16>(data, offset);
+
+			provider.services.append(service);
+			num_services--;
+		}
+
+		msg.providers.append(provider);
+		num_records--;
+	}
+
+	return msg;
+}
+
+QByteArray SAProtocol::packCapabilityDiscoveryResponse(const CapabilityDiscoveryResponse &msg)
+{
+	QByteArray data;
+
+	append<quint8>(data, msg.messageType);
+	append<quint8>(data, msg.queryType);
+	append<quint32>(data, msg.checksum);
+
+	append<quint16>(data, msg.providers.size());
+
+	foreach (const CapabilityDiscoveryProvider &provider, msg.providers) {
+		append<quint16>(data, provider.uuid);
+		data.append(provider.name.toUtf8());
+		data.append(';');
+		append<quint16>(data, provider.services.size());
+
+		foreach (const CapabilityDiscoveryService &service, provider.services) {
+			append<quint16>(data, service.componentId);
+			data.append(service.profile.toUtf8());
+			data.append(';');
+			append<quint16>(data, service.aspVersion);
+			append<quint8>(data, service.role);
+			append<quint16>(data, service.connTimeout);
+		}
+	}
+
+	return data;
+}
diff --git a/saprotocol.h b/saprotocol.h
new file mode 100644
index 0000000..469a293
--- /dev/null
+++ b/saprotocol.h
@@ -0,0 +1,195 @@
+#ifndef SAPROTOCOL_H
+#define SAPROTOCOL_H
+
+#include <QtCore/QObject>
+
+#if QT_VERSION >= QT_VERSION_CHECK(5,0,0)
+#include <QtBluetooth/QBluetoothUuid>
+QT_USE_NAMESPACE_BLUETOOTH
+#else
+#include <QtConnectivity/QBluetoothUuid>
+QTM_USE_NAMESPACE
+#endif
+
+class SAProtocol
+{
+	Q_GADGET
+	Q_ENUMS(Role FrameType ControlFrameType)
+
+public:
+	static const QBluetoothUuid dataServiceUuid;
+	static const QBluetoothUuid nudgeServiceUuid;
+
+	enum Role {
+		ClientRole,
+		ServerRole
+	};
+
+	static const quint8 currentProtocolVersion = 0;
+
+	static const quint16 defaultSessionId = 0x3FF;
+	static const quint16 maxSessionId = 0x3FF;
+
+	static quint16 computeCrc16(const QByteArray &buf);
+
+	static const char peerDescriptionSeparator = ';';
+
+	/** One of the early connection setup messages. Seems to describe each
+	 *  other's capabilities and network settings to the remote peer. */
+	struct PeerDescription {
+		/** No idea: either 5 or 6. Wild guess: 5 is request, 6 is response. */
+		quint8 messageType;
+		quint16 accessoryProtocolVersion;
+		quint16 accessorySoftwareVersion;
+		/** Status code. 0 generally means "no error". */
+		quint8 status;
+		quint8 errorCode; /**< Used when status != 0 and messageType == 6 */
+
+		/* Seemingly network parameters, but I am not sure how the other end
+		 * actually handles them. Seem to do nothing. */
+		quint32 APDUSize;
+		quint16 SSDUSize;
+		quint16 sessions; /**< Seemingly maximum number of sessions allowed. */
+		quint16 timeout; /**< Looks like a timeout, but no idea what for. */
+
+		/* No idea. */
+		quint8 unk_1;
+		quint16 unk_2;
+		quint8 unk_3;
+
+		/* No one cares about the ones below. Seemingly free-form text. */
+		QString product;
+		QString manufacturer;
+		QString name;
+		QString profile;
+	};
+
+	static PeerDescription unpackPeerDescription(const QByteArray &data);
+	static QByteArray packPeerDescription(const PeerDescription &desc);
+
+	enum SecurityFrameType {
+		SecurityAuthenticateRequest = 0x10,
+		SecurityAuthenticateResponse = 0x11,
+		SecurityAuthenticateConfirm = 0x12
+	};
+
+	struct SecurityFrame {
+		SecurityFrameType type;
+		quint8 authType; // ???
+		quint8 version; // Always 0 so far.
+		quint8 dataType; // ???
+		QByteArray data;
+	};
+
+	static SecurityFrame unpackSecurityFrame(const QByteArray &data);
+	static QByteArray packSecurityFrame(const SecurityFrame &sframe);
+
+	enum FrameType {
+		FrameData = 0,
+		FrameControl = 1
+	};
+
+	struct Frame {
+		quint8 protocolVersion;
+		FrameType type;
+		quint16 sessionId;
+		QByteArray data;
+	};
+
+	static Frame unpackFrame(const QByteArray &data);
+	static QByteArray packFrame(const Frame& frame);
+	static QByteArray packFrame(quint16 sessionId, const QByteArray &data);
+
+	enum DefaultSessionMessageType {
+		ServiceConnectionRequest = 1,
+		ServiceConnectionResponse = 2,
+		ServiceTerminationRequest = 3,
+		ServiceTerminationResponse = 4
+	};
+
+	/** The following settings map 1:1 to contents in service.xml files (thank god).
+	 *  That does not mean I understand what they do, specially the QoS part. */
+
+	struct ServiceConnectionRequestSession {
+		quint16 sessionId;
+		quint16 channelId;
+		quint8 qosType;
+		quint8 qosDataRate;
+		quint8 qosPriority;
+		quint8 payloadType;
+	};
+
+	struct ServiceConnectionRequestFrame {
+		quint8 messageType;
+		quint16 acceptorId;
+		quint16 initiatorId;
+		QString profile;
+		QList<ServiceConnectionRequestSession> sessions;
+	};
+
+	static ServiceConnectionRequestFrame unpackServiceConnectionRequestFrame(const QByteArray &data);
+	static QByteArray packServiceConnectionRequestFrame(const ServiceConnectionRequestFrame &frame);
+
+	struct ServiceConnectionResponseFrame {
+		quint8 messageType;
+		quint16 acceptorId;
+		quint16 initiatorId;
+		QString profile;
+		quint8 statusCode;
+		QList<quint16> sessions;
+	};
+
+	static ServiceConnectionResponseFrame unpackServiceConnectionResponseFrame(const QByteArray &data);
+	static QByteArray packServiceConnectionResponseFrame(const ServiceConnectionResponseFrame &frame);
+
+	// TODO service conn termination request. probably trivial.
+
+	static const QLatin1String capabilityDiscoveryProfile;
+	static const quint16 capabilityDiscoveryChannel = 255;
+	static const quint16 capabilityDiscoveryAgentId = 0xFFFF;
+
+	enum CapabilityDiscoveryMessageType {
+		CapabilityDiscoveryMessageTypeQuery = 1,
+		CapabilityDiscoveryMessageTypeResponse = 2
+	};
+
+	struct CapabilityDiscoveryQuery {
+		CapabilityDiscoveryMessageType messageType;
+		quint8 queryType;
+		quint32 checksum;
+		QList<QString> records;
+	};
+
+	static CapabilityDiscoveryQuery unpackCapabilityDiscoveryQuery(const QByteArray &data);
+	static QByteArray packCapabilityDiscoveryQuery(const CapabilityDiscoveryQuery &msg);
+
+	struct CapabilityDiscoveryService {
+		quint16 componentId;
+		quint16 aspVersion;
+		quint8 role;
+		quint16 connTimeout;
+		QString profile;
+	};
+
+	struct CapabilityDiscoveryProvider {
+		quint16 uuid;
+		QString name;
+		QList<CapabilityDiscoveryService> services;
+	};
+
+	struct CapabilityDiscoveryResponse {
+		CapabilityDiscoveryMessageType messageType;
+		quint8 queryType;
+		quint32 checksum;
+		QList<CapabilityDiscoveryProvider> providers;
+	};
+
+	static CapabilityDiscoveryResponse unpackCapabilityDiscoveryResponse(const QByteArray &data);
+	static QByteArray packCapabilityDiscoveryResponse(const CapabilityDiscoveryResponse &msg);
+
+private:
+	Q_DISABLE_COPY(SAProtocol)
+	SAProtocol();
+};
+
+#endif // SAPROTOCOL_H
diff --git a/sapserviceinfo.cc b/sapserviceinfo.cc
new file mode 100644
index 0000000..a77d2f0
--- /dev/null
+++ b/sapserviceinfo.cc
@@ -0,0 +1,106 @@
+#include <QtCore/QMap>
+#include <QtCore/QSharedData>
+
+#include "sapserviceinfo.h"
+#include "sapchannelinfo.h"
+
+
+class SAPServiceInfoData : public QSharedData {
+public:
+	QString profile;
+	QString friendlyName;
+	SAPServiceInfo::Role role;
+	unsigned short version;
+	unsigned short connectionTimeout;
+
+	QMap<int, SAPChannelInfo> channels;
+};
+
+SAPServiceInfo::SAPServiceInfo() : data(new SAPServiceInfoData)
+{
+}
+
+SAPServiceInfo::SAPServiceInfo(const SAPServiceInfo &rhs) : data(rhs.data)
+{
+}
+
+SAPServiceInfo &SAPServiceInfo::operator=(const SAPServiceInfo &rhs)
+{
+	if (this != &rhs)
+		data.operator=(rhs.data);
+	return *this;
+}
+
+SAPServiceInfo::~SAPServiceInfo()
+{
+}
+
+QString SAPServiceInfo::profile() const
+{
+	return data->profile;
+}
+
+void SAPServiceInfo::setProfile(const QString &profile)
+{
+	data->profile = profile;
+}
+
+QString SAPServiceInfo::friendlyName() const
+{
+	return data->friendlyName;
+}
+
+void SAPServiceInfo::setFriendlyName(const QString &name)
+{
+	data->friendlyName = name;
+}
+
+SAPServiceInfo::Role SAPServiceInfo::role() const
+{
+	return data->role;
+}
+
+void SAPServiceInfo::setRole(SAPServiceInfo::Role role)
+{
+	data->role = role;
+}
+
+unsigned short SAPServiceInfo::version() const
+{
+	return data->version;
+}
+
+void SAPServiceInfo::setVersion(unsigned short version)
+{
+	data->version = version;
+}
+
+void SAPServiceInfo::setVersion(unsigned char maj, unsigned char min)
+{
+	setVersion((maj << 8) | (min & 0xFF));
+}
+
+unsigned short SAPServiceInfo::connectionTimeout() const
+{
+	return data->connectionTimeout;
+}
+
+void SAPServiceInfo::setConnectionTimeout(unsigned short timeout)
+{
+	data->connectionTimeout = timeout;
+}
+
+void SAPServiceInfo::addChannel(const SAPChannelInfo &channel)
+{
+	data->channels.insert(channel.channelId(), channel);
+}
+
+void SAPServiceInfo::removeChannel(unsigned short channelId)
+{
+	data->channels.remove(channelId);
+}
+
+QList<SAPChannelInfo> SAPServiceInfo::channels() const
+{
+	return data->channels.values();
+}
diff --git a/sapserviceinfo.h b/sapserviceinfo.h
new file mode 100644
index 0000000..91756b7
--- /dev/null
+++ b/sapserviceinfo.h
@@ -0,0 +1,64 @@
+#ifndef SAPSERVICEINFO_H
+#define SAPSERVICEINFO_H
+
+#include <QtCore/QObject>
+#include <QtCore/QSharedDataPointer>
+
+class SAPServiceInfoData;
+class SAPChannelInfo;
+
+class SAPServiceInfo
+{
+	Q_GADGET
+	Q_ENUMS(Role)
+
+public:
+	SAPServiceInfo();
+	SAPServiceInfo(const SAPServiceInfo &);
+	SAPServiceInfo &operator=(const SAPServiceInfo &);
+	~SAPServiceInfo();
+
+	enum Role {
+		RoleProvider,
+		RoleConsumer
+	};
+
+	static Role oppositeRole(Role role);
+
+	QString profile() const;
+	void setProfile(const QString &profile);
+
+	QString friendlyName() const;
+	void setFriendlyName(const QString &name);
+
+	Role role() const;
+	void setRole(Role role);
+
+	unsigned short version() const;
+	void setVersion(unsigned short version);
+	void setVersion(unsigned char maj, unsigned char min);
+
+	unsigned short connectionTimeout() const;
+	void setConnectionTimeout(unsigned short timeout);
+
+	void addChannel(const SAPChannelInfo &channel);
+	void removeChannel(unsigned short channelId);
+	QList<SAPChannelInfo> channels() const;
+
+private:
+	QSharedDataPointer<SAPServiceInfoData> data;
+};
+
+inline SAPServiceInfo::Role SAPServiceInfo::oppositeRole(Role role)
+{
+	switch (role) {
+	case RoleProvider:
+		return RoleConsumer;
+	case RoleConsumer:
+		return RoleProvider;
+	default:
+		abort();
+	}
+}
+
+#endif // SAPSERVICEINFO_H
diff --git a/sapsocket.cc b/sapsocket.cc
new file mode 100644
index 0000000..ce85c0c
--- /dev/null
+++ b/sapsocket.cc
@@ -0,0 +1,62 @@
+#include <QtCore/QDebug>
+
+#include "sappeer.h"
+#include "sapconnection.h"
+#include "sapsocket.h"
+
+SAPSocket::SAPSocket(SAPConnection *conn, int sessionId) :
+    QObject(conn), _sessionId(sessionId), _open(false)
+{
+}
+
+SAPPeer * SAPSocket::peer()
+{
+	return connection()->peer();
+}
+
+SAPConnection * SAPSocket::connection()
+{
+	return static_cast<SAPConnection*>(parent());
+}
+
+bool SAPSocket::isOpen() const
+{
+	return _open;
+}
+
+bool SAPSocket::messageAvailable() const
+{
+	return !_in.empty();
+}
+
+QByteArray SAPSocket::receive()
+{
+	if (!_in.empty()) {
+		return _in.dequeue();
+	} else {
+		return QByteArray();
+	}
+}
+
+bool SAPSocket::send(const QByteArray &data)
+{
+	return peer()->writeToSession(_sessionId, data);
+}
+
+void SAPSocket::setOpen(bool open)
+{
+	_open = open;
+}
+
+void SAPSocket::acceptIncomingData(const QByteArray &data)
+{
+	if (data.isEmpty()) return;
+	_in.enqueue(data);
+
+	emit messageReceived();
+}
+
+int SAPSocket::sessionId() const
+{
+	return _sessionId;
+}
diff --git a/sapsocket.h b/sapsocket.h
new file mode 100644
index 0000000..e1a9214
--- /dev/null
+++ b/sapsocket.h
@@ -0,0 +1,45 @@
+#ifndef SAPSOCKET_H
+#define SAPSOCKET_H
+
+#include <QtCore/QObject>
+#include <QtCore/QQueue>
+
+class SAPConnection;
+class SAPPeer;
+
+class SAPSocket : public QObject
+{
+	Q_OBJECT
+
+	SAPSocket(SAPConnection *conn, int sessionId);
+
+public:
+	SAPPeer *peer();
+	SAPConnection *connection();
+
+	bool isOpen() const;
+
+	bool messageAvailable() const;
+	QByteArray receive();
+	bool send(const QByteArray &data);
+
+signals:
+	void connected();
+	void disconnected();
+	void messageReceived();
+
+protected:
+	void setOpen(bool open);
+	void acceptIncomingData(const QByteArray &data);
+
+	int sessionId() const;
+
+private:
+	int _sessionId;
+	bool _open;
+	QQueue<QByteArray> _in;
+
+	friend class SAPPeer;
+};
+
+#endif // SAPSOCKET_H
diff --git a/scripts/test-hfp-ag b/scripts/test-hfp-ag
new file mode 100755
index 0000000..3152339
--- /dev/null
+++ b/scripts/test-hfp-ag
@@ -0,0 +1,210 @@
+#!/usr/bin/python
+
+from __future__ import absolute_import, print_function, unicode_literals
+
+from optparse import OptionParser, make_option
+import os
+from socket import SOCK_SEQPACKET, socket
+import sys
+import dbus
+import dbus.service
+import dbus.mainloop.glib
+import glib
+try:
+  from gi.repository import GObject
+except ImportError:
+  import gobject as GObject
+
+mainloop = None
+audio_supported = True
+
+try:
+	from socket import AF_BLUETOOTH, BTPROTO_SCO
+except:
+	print("WARNING: python compiled without Bluetooth support"
+					" - audio will not be available")
+	audio_supported = False
+
+BUF_SIZE = 1024
+
+BDADDR_ANY = '00:00:00:00:00:00'
+
+HF_NREC			= 0x0001
+HF_3WAY			= 0x0002
+HF_CLI			= 0x0004
+HF_VOICE_RECOGNITION	= 0x0008
+HF_REMOTE_VOL		= 0x0010
+HF_ENHANCED_STATUS	= 0x0020
+HF_ENHANCED_CONTROL	= 0x0040
+HF_CODEC_NEGOTIATION	= 0x0080
+
+AG_3WAY			= 0x0001
+AG_NREC			= 0x0002
+AG_VOICE_RECOGNITION	= 0x0004
+AG_INBAND_RING		= 0x0008
+AG_VOICE_TAG		= 0x0010
+AG_REJECT_CALL		= 0x0020
+AG_ENHANCED_STATUS	= 0x0040
+AG_ENHANCED_CONTROL	= 0x0080
+AG_EXTENDED_RESULT	= 0x0100
+AG_CODEC_NEGOTIATION	= 0x0200
+
+HF_FEATURES = (HF_3WAY | HF_CLI | HF_VOICE_RECOGNITION |
+			HF_REMOTE_VOL | HF_ENHANCED_STATUS |
+			HF_ENHANCED_CONTROL | HF_CODEC_NEGOTIATION)
+
+AVAIL_CODECS = "1,2"
+
+class HfpConnection:
+	slc_complete = False
+	fd = None
+	io_id = 0
+	version = 0
+	features = 0
+	pending = None
+
+	def disconnect(self):
+		if (self.fd >= 0):
+			os.close(self.fd)
+			self.fd = -1
+			glib.source_remove(self.io_id)
+			self.io_id = 0
+
+	def slc_completed(self):
+		print("SLC establisment complete")
+		self.slc_complete = True
+
+	def slc_next_cmd(self, cmd):
+		print("Unknown SLC command completed: %s" % (cmd))
+
+	def io_cb(self, fd, cond):
+		buf = os.read(fd, BUF_SIZE)
+		buf = buf.strip()
+
+		print("Received: %s" % (buf))
+
+		return True
+
+	def send_cmd(self, cmd):
+		if (self.pending):
+			print("ERROR: Another command is pending")
+			return
+
+		print("Sending: %s" % (cmd))
+
+		os.write(self.fd, cmd + "\r\n")
+		self.pending = cmd
+
+	def __init__(self, fd, version, features):
+		self.fd = fd
+		self.version = version
+		self.features = features
+
+		print("Version 0x%04x Features 0x%04x" % (version, features))
+
+		self.io_id = glib.io_add_watch(fd, glib.IO_IN, self.io_cb)
+
+class HfpProfile(dbus.service.Object):
+	sco_socket = None
+	io_id = 0
+	conns = {}
+
+	def sco_cb(self, sock, cond):
+		(sco, peer) = sock.accept()
+		print("New SCO connection from %s" % (peer))
+
+	def init_sco(self, sock):
+		self.sco_socket = sock
+		self.io_id = glib.io_add_watch(sock, glib.IO_IN, self.sco_cb)
+
+	def __init__(self, bus, path, sco):
+		dbus.service.Object.__init__(self, bus, path)
+
+		if sco:
+			self.init_sco(sco)
+
+	@dbus.service.method("org.bluez.Profile1",
+					in_signature="", out_signature="")
+	def Release(self):
+		print("Release")
+		mainloop.quit()
+
+	@dbus.service.method("org.bluez.Profile1",
+					in_signature="", out_signature="")
+	def Cancel(self):
+		print("Cancel")
+
+	@dbus.service.method("org.bluez.Profile1",
+				in_signature="o", out_signature="")
+	def RequestDisconnection(self, path):
+		conn = self.conns.pop(path)
+		conn.disconnect()
+
+	@dbus.service.method("org.bluez.Profile1",
+				in_signature="oha{sv}", out_signature="")
+	def NewConnection(self, path, fd, properties):
+		fd = fd.take()
+		version = 0x0105
+		features = 0
+		print("NewConnection(%s, %d)" % (path, fd))
+		for key in properties.keys():
+			if key == "Version":
+				version = properties[key]
+			elif key == "Features":
+				features = properties[key]
+
+		#conn = HfpConnection(fd, version, features)
+
+		#self.conns[path] = conn
+
+if __name__ == '__main__':
+	dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
+
+	bus = dbus.SystemBus()
+
+	manager = dbus.Interface(bus.get_object("org.bluez",
+				"/org/bluez"), "org.bluez.ProfileManager1")
+
+	option_list = [
+			make_option("-p", "--path", action="store",
+					type="string", dest="path",
+					default="/bluez/test/hfp"),
+			make_option("-n", "--name", action="store",
+					type="string", dest="name",
+					default=None),
+			make_option("-C", "--channel", action="store",
+					type="int", dest="channel",
+					default=None),
+			]
+
+	parser = OptionParser(option_list=option_list)
+
+	(options, args) = parser.parse_args()
+
+	mainloop = GObject.MainLoop()
+
+	opts = {
+			"Version" : dbus.UInt16(0x0106),
+			"Features" : dbus.UInt16(HF_FEATURES),
+		}
+
+	if (options.name):
+		opts["Name"] = options.name
+
+	if (options.channel is not None):
+		opts["Channel"] = dbus.UInt16(options.channel)
+
+	if audio_supported:
+		sco = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO)
+		sco.bind(BDADDR_ANY)
+		sco.listen(1)
+	else:
+		sco = None
+
+	profile = HfpProfile(bus, options.path, sco)
+
+	manager.RegisterProfile(options.path, "hfp-ag", opts)
+
+	print("Profile registered - waiting for connections")
+
+	mainloop.run()
diff --git a/wmscrypt.h b/wmscrypt.h
new file mode 100644
index 0000000..c7271cb
--- /dev/null
+++ b/wmscrypt.h
@@ -0,0 +1,15 @@
+#ifndef WMSCRYPT_H
+#define WMSCRYPT_H
+
+#include <QtCore/QtGlobal>
+
+namespace wms
+{
+
+void decrypt_block(quint8 *dst, const quint8 *src);
+void decrypt_block_cbc(quint8 *dst, const quint8 *src, int len, const quint8 *iv);
+
+}
+
+
+#endif // WMSCRYPT_H
diff --git a/wmskeys.h b/wmskeys.h
new file mode 100644
index 0000000..5170ee8
--- /dev/null
+++ b/wmskeys.h
@@ -0,0 +1,21 @@
+#ifndef WMSKEYS_H
+#define WMSKEYS_H
+
+extern unsigned char key_DecT0[];
+extern unsigned char key_DecT1[];
+extern unsigned char key_DecT2[];
+extern unsigned char key_DecTin[];
+extern unsigned char key_DecTout[];
+
+extern unsigned char key_finalT0[];
+extern unsigned char key_finalT1[];
+
+extern unsigned char key_psk_table[];
+
+extern unsigned char key_T0[];
+extern unsigned char key_T1[];
+extern unsigned char key_T2[];
+extern unsigned char key_Tin[];
+extern unsigned char key_Tout[];
+
+#endif // WMSKEYS_H
diff --git a/wmspeer.h b/wmspeer.h
new file mode 100644
index 0000000..446e747
--- /dev/null
+++ b/wmspeer.h
@@ -0,0 +1,46 @@
+#ifndef WMSPEER_H
+#define WMSPEER_H
+
+#include <QtCore/QObject>
+#include <openssl/ec.h>
+
+#include "saprotocol.h"
+
+class WMSPeer : public QObject
+{
+	Q_OBJECT
+
+public:
+	WMSPeer(SAProtocol::Role role, const QString &localName, const QString &peerName, QObject *parent = 0);
+	~WMSPeer();
+
+	SAProtocol::SecurityFrame respondToServerChallenge(const SAProtocol::SecurityFrame &challenge);
+	bool verifyServerResponse(const SAProtocol::SecurityFrame &challenge);
+
+private:
+	bool generateEccKey();
+
+	QByteArray computeSharedSecret(const QByteArray &remotePubKey) const;
+	QByteArray expandTemporaryKey();
+
+	static QByteArray getExpandedPskKey(quint16 index);
+
+	static QByteArray sha256(const QByteArray &message);
+
+private:
+	EC_KEY *_key;
+	QByteArray _pubKey;
+	quint32 _id;
+
+	QString _serverName;
+	QString _clientName;
+
+	QByteArray _sharedSecret;
+
+	quint16 _clientTmpNum;
+	QByteArray _clientTmpKey;
+	quint16 _serverTmpNum;
+	QByteArray _serverTmpKey;
+};
+
+#endif // WMSPEER_H