reorganize source files into SAP and agents

31 files changed, 3388 insertions, 0 deletions

diff --git a/sap/capabilityagent.cc b/sap/capabilityagent.cc
new file mode 100644
index 0000000..cdc80b6
--- /dev/null
+++ b/sap/capabilityagent.cc
@@ -0,0 +1,69 @@
+#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)
+{
+	// We make the capability peer a child of the peer object itself,
+	// so that the peer can find it.
+	CapabilityPeer *capPeer = new CapabilityPeer(peer, peer);
+	connect(peer, SIGNAL(disconnected()), capPeer, SLOT(deleteLater()));
+}
+
+void CapabilityAgent::requestConnection(SAPConnectionRequest *request)
+{
+	SAPPeer *peer = request->peer();
+	CapabilityPeer *capPeer = peer->findChild<CapabilityPeer*>();
+
+	Q_ASSERT(capPeer);
+
+	capPeer->requestConnection(request);
+}
diff --git a/sap/capabilityagent.h b/sap/capabilityagent.h
new file mode 100644
index 0000000..52692e5
--- /dev/null
+++ b/sap/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/sap/capabilitypeer.cc b/sap/capabilitypeer.cc
new file mode 100644
index 0000000..8dfe603
--- /dev/null
+++ b/sap/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" << int(data[0]) << "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/sap/capabilitypeer.h b/sap/capabilitypeer.h
new file mode 100644
index 0000000..0ad6fc6
--- /dev/null
+++ b/sap/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/sap/crc16.cc b/sap/crc16.cc
new file mode 100644
index 0000000..f131957
--- /dev/null
+++ b/sap/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/sap/crc16.h b/sap/crc16.h
new file mode 100644
index 0000000..f6591e9
--- /dev/null
+++ b/sap/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/sap/endianhelpers.h b/sap/endianhelpers.h
new file mode 100644
index 0000000..3060382
--- /dev/null
+++ b/sap/endianhelpers.h
@@ -0,0 +1,27 @@
+#ifndef ENDIANHELPERS_H
+#define ENDIANHELPERS_H
+
+#include <QtCore/QtEndian>
+
+namespace
+{
+
+template<typename T>
+inline T read(const QByteArray &data, int &offset)
+{
+	T unswapped;
+	memcpy(&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));
+}
+
+}
+
+#endif // ENDIANHELPERS_H
diff --git a/sap/sapagent.h b/sap/sapagent.h
new file mode 100644
index 0000000..321075c
--- /dev/null
+++ b/sap/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/sap/sapbtlistener.cc b/sap/sapbtlistener.cc
new file mode 100644
index 0000000..9776b59
--- /dev/null
+++ b/sap/sapbtlistener.cc
@@ -0,0 +1,182 @@
+#include <QtCore/QDebug>
+
+#include "saprotocol.h"
+#include "sapbtlistener.h"
+#include "sapbtpeer.h"
+
+#ifdef SAILFISH
+#include <bluez-qt5/bluemanager.h>
+#include <bluez-qt5/blueadapter.h>
+#include <bluez-qt5/bluedevice.h>
+#include <bluez-qt5/headset.h>
+#endif
+#ifdef DESKTOP
+#include "hfpag.h"
+#endif
+
+SAPBTListener::SAPBTListener(QObject *parent) :
+    QObject(parent), _server(0)
+{
+}
+
+SAPBTListener::~SAPBTListener()
+{
+	stop();
+}
+
+void SAPBTListener::start()
+{
+	if (_server) {
+		qWarning() << "Already started";
+		return;
+	}
+
+	_server = new QBluetoothServer(QBluetoothServiceInfo::RfcommProtocol, 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();
+
+	/*
+	 	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(SAProtocol::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";
+	}
+}
+
+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(QBluetoothServiceInfo::RfcommProtocol, this);
+
+	connect(socket, SIGNAL(connected()), this, SLOT(handleNudgeConnected()));
+	connect(socket, SIGNAL(error(QBluetoothSocket::SocketError)),
+	        this, SLOT(handleNudgeError(QBluetoothSocket::SocketError)));
+
+	qDebug() << "Nudging" << address.toString();
+
+	// First, set up HFP/Headset connection to watch
+#if SAILFISH
+	QDBusConnection bus = QDBusConnection::systemBus();
+	OrgBluezManagerInterface manager("org.bluez", "/", bus);
+	QDBusReply<QDBusObjectPath> defaultAdapter = manager.DefaultAdapter();
+	if (!defaultAdapter.isValid()) {
+		qWarning() << "Could not get default Bluez adapter:" << defaultAdapter.error().message();
+	}
+	OrgBluezAdapterInterface adapter("org.bluez", defaultAdapter.value().path(), bus);
+	QList<QDBusObjectPath> list = adapter.ListDevices();
+	foreach (QDBusObjectPath item, list) {
+		OrgBluezDeviceInterface device("org.bluez", item.path(), bus);
+		QVariantMap properties = device.GetProperties();
+		QBluetoothAddress devAddress(properties["Address"].toString());
+		if (devAddress == address) {
+			OrgBluezHeadsetInterface headset("org.bluez", item.path(), bus);
+			qDebug() << "Creating HFP connection to" << devAddress.toString();
+			headset.Connect();
+		}
+	}
+#elif DESKTOP
+	new HfpAg(address, this);
+#endif
+
+	// After that, start normal connection.
+#if SAILFISH
+	// For some reason, using UUIDs here fails on SailfishOS
+	socket->connectToService(address, 1);
+#else
+	socket->connectToService(address, SAProtocol::nudgeServiceUuid);
+#endif
+}
+
+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
+	SAPBTPeer *peer = new SAPBTPeer(SAProtocol::ClientRole, socket, this);
+	connect(peer, SIGNAL(disconnected()), peer, SLOT(deleteLater()));
+}
+
+void SAPBTListener::handleNudgeConnected()
+{
+	QBluetoothSocket *socket = static_cast<QBluetoothSocket*>(sender());
+	qDebug() << "Nudge connected:" << socket->peerAddress().toString();
+	new SAPBTPeer(SAProtocol::ClientRole, socket, this);
+}
+
+void SAPBTListener::handleNudgeError(QBluetoothSocket::SocketError error)
+{
+	QBluetoothSocket *socket = static_cast<QBluetoothSocket*>(sender());
+	qWarning() << "Cannot nudge:" << error << socket->errorString();
+	socket->abort();
+	socket->deleteLater();
+}
diff --git a/sap/sapbtlistener.h b/sap/sapbtlistener.h
new file mode 100644
index 0000000..398eb1a
--- /dev/null
+++ b/sap/sapbtlistener.h
@@ -0,0 +1,35 @@
+#ifndef SAPBTLISTENER_H
+#define SAPBTLISTENER_H
+
+#include <QtCore/QObject>
+#include <QtBluetooth/QBluetoothServer>
+#include <QtBluetooth/QBluetoothServiceInfo>
+
+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();
+    void handleNudgeConnected();
+	void handleNudgeError(QBluetoothSocket::SocketError error);
+
+private:
+	QBluetoothServer *_server;
+	QBluetoothServiceInfo _service;
+};
+
+#endif // SAPBTLISTENER_H
diff --git a/sap/sapbtpeer.cc b/sap/sapbtpeer.cc
new file mode 100644
index 0000000..188c37b
--- /dev/null
+++ b/sap/sapbtpeer.cc
@@ -0,0 +1,191 @@
+#include <QtCore/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) {
+			// We are waiting for a full frame of known length
+			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;
+		qDebug() << "apdu=" << peerDesc.APDUSize << "ssdu=" << peerDesc.SSDUSize
+				 << "sessions=" << peerDesc.sessions << "timeout=" << peerDesc.timeout;
+
+		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:
+			handleControlFrame(frame);
+			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::handleControlFrame(const SAProtocol::Frame &frame)
+{
+	Q_ASSERT(frame.type == SAProtocol::FrameControl);
+
+	handleSessionControl(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/sap/sapbtpeer.h b/sap/sapbtpeer.h
new file mode 100644
index 0000000..75af77f
--- /dev/null
+++ b/sap/sapbtpeer.h
@@ -0,0 +1,36 @@
+#ifndef SAPBTPEER_H
+#define SAPBTPEER_H
+
+#include <QtCore/QObject>
+#include <QtBluetooth/QBluetoothSocket>
+
+#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 handleControlFrame(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/sap/sapchannelinfo.cc b/sap/sapchannelinfo.cc
new file mode 100644
index 0000000..530341a
--- /dev/null
+++ b/sap/sapchannelinfo.cc
@@ -0,0 +1,89 @@
+#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;
+}
+
+QDebug operator<<(QDebug debug, const SAPChannelInfo &info)
+{
+	QDebugStateSaver saver(debug);
+	Q_UNUSED(saver);
+	debug.nospace() << "SAPChannelInfo(" << info.channelId() << ", qosType=" << info.qosType() << ", dataRate=" << info.qosDataRate() << ", priority=" << info.qosPriority()
+					<< ", payload=" << info.payloadType() << ")";
+	return debug;
+}
diff --git a/sap/sapchannelinfo.h b/sap/sapchannelinfo.h
new file mode 100644
index 0000000..922136e
--- /dev/null
+++ b/sap/sapchannelinfo.h
@@ -0,0 +1,68 @@
+#ifndef SAPCHANNELINFO_H
+#define SAPCHANNELINFO_H
+
+#include <QtCore/QObject>
+#include <QtCore/QSharedDataPointer>
+#include <QtCore/QDebug>
+
+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;
+};
+
+QDebug operator<<(QDebug debug, const SAPChannelInfo &info);
+
+#endif // SAPCHANNELINFO_H
diff --git a/sap/sapconnection.cc b/sap/sapconnection.cc
new file mode 100644
index 0000000..9a624b6
--- /dev/null
+++ b/sap/sapconnection.cc
@@ -0,0 +1,33 @@
+#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::setSocket(int channelId, SAPSocket *socket)
+{
+	Q_ASSERT(!_sockets.contains(channelId));
+	_sockets.insert(channelId, socket);
+}
diff --git a/sap/sapconnection.h b/sap/sapconnection.h
new file mode 100644
index 0000000..0fdd025
--- /dev/null
+++ b/sap/sapconnection.h
@@ -0,0 +1,40 @@
+#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();
+
+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/sap/sapconnectionrequest.cc b/sap/sapconnectionrequest.cc
new file mode 100644
index 0000000..4842ff6
--- /dev/null
+++ b/sap/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/sap/sapconnectionrequest.h b/sap/sapconnectionrequest.h
new file mode 100644
index 0000000..bbb4a0e
--- /dev/null
+++ b/sap/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/sap/sapmanager.cc b/sap/sapmanager.cc
new file mode 100644
index 0000000..9d72433
--- /dev/null
+++ b/sap/sapmanager.cc
@@ -0,0 +1,160 @@
+#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() const
+{
+	return QSet<QString>::fromList(_consumerProfiles.keys())
+	        + QSet<QString>::fromList(_providerProfiles.keys());
+}
+
+QSet<SAPAgent*> SAPManager::allAgents() const
+{
+	QSet<SAPAgent*> agents;
+	foreach (const RegisteredAgent &ragent, _agents) {
+		agents.insert(ragent.agent);
+	}
+	return agents;
+}
+
+void SAPManager::registerApplicationPackage(const RegisteredApplication &app)
+{
+	_pkgs.insert(app.package, app);
+}
+
+void SAPManager::registerApplicationPackage(const QString &package, const QString &name, int version, bool preinstalled)
+{
+	RegisteredApplication app;
+	app.package = package;
+	app.name = name;
+	if (app.name.isEmpty()) app.name = package;
+	app.version = version;
+	app.preinstalled = preinstalled;
+	registerApplicationPackage(app);
+}
+
+void SAPManager::unregisterApplicationPackage(const QString &package)
+{
+	_pkgs.remove(package);
+}
+
+QList<SAPManager::RegisteredApplication> SAPManager::allPackages() const
+{
+	return _pkgs.values();
+}
+
+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/sap/sapmanager.h b/sap/sapmanager.h
new file mode 100644
index 0000000..5619e54
--- /dev/null
+++ b/sap/sapmanager.h
@@ -0,0 +1,66 @@
+#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() const;
+	QSet<SAPAgent*> allAgents() const;
+
+	struct RegisteredApplication {
+		QString package;
+		QString name;
+		int version;
+		bool preinstalled;
+	};
+
+	void registerApplicationPackage(const RegisteredApplication &app);
+	void registerApplicationPackage(const QString &package, const QString &name = QString(), int version = 1, bool preinstalled = false);
+	void unregisterApplicationPackage(const QString &package);
+
+	QList<RegisteredApplication> allPackages() const;
+
+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;
+
+	QHash<QString, RegisteredApplication> _pkgs;
+};
+
+#endif // SAPMANAGER_H
diff --git a/sap/sappeer.cc b/sap/sappeer.cc
new file mode 100644
index 0000000..85a5899
--- /dev/null
+++ b/sap/sappeer.cc
@@ -0,0 +1,431 @@
+#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)
+{
+}
+
+SAPPeer::~SAPPeer()
+{
+	qDeleteAll(_conns);
+	qDeleteAll(_sessions);
+	_conns.clear();
+	_sessions.clear();
+}
+
+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, cInfo);
+		conn->setSocket(session.channelId, socket);
+
+		_sessions.insert(sessionId, socket);
+	}
+
+	_conns.insert(profile, conn);
+
+	writeDataToSession(SAProtocol::defaultSessionId,
+					   SAProtocol::packServiceConnectionRequestFrame(request));
+
+	return conn;
+}
+
+bool SAPPeer::terminateServiceConnection(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 false;
+	}
+
+	// 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 false;
+	}
+
+	SAPConnection *conn = _conns.value(profile, 0);
+	if (conn) {
+		SAProtocol::ServiceTerminationRequestFrame request;
+		request.messageType = SAProtocol::ServiceTerminationRequest;
+		request.initiatorId = initiator;
+		request.acceptorId = acceptor;
+		request.profile = profile;
+
+		writeDataToSession(SAProtocol::defaultSessionId,
+						   SAProtocol::packServiceTerminationRequestFrame(request));
+
+		return true;
+	} else {
+		qWarning() << "Connection does not exist:" << profile;
+		return false;
+	}
+}
+
+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*>();
+}
+
+void SAPPeer::writeDataToSession(int session, const QByteArray &data)
+{
+	sendFrame(SAProtocol::packFrame(session, data, SAProtocol::FrameData));
+}
+
+void SAPPeer::writeControlToSession(int session, const QByteArray &data)
+{
+	sendFrame(SAProtocol::packFrame(session, data, SAProtocol::FrameControl));
+}
+
+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() << "Accepting service conection with status" << statusCode;
+
+	writeDataToSession(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());
+		}
+		_conns.remove(conn->profile());
+		delete conn;
+	}
+
+	// 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::handleSessionControl(int session, const QByteArray &data)
+{
+	// Default session ID should not receive control messages, so we don't check for it
+	SAPSocket *socket = _sessions.value(session, 0);
+	if (socket && socket->isOpen()) {
+		socket->acceptIncomingControl(data);
+	} else {
+		qWarning() << "Got information for a session that's not yet open!" << session;
+	}
+}
+
+void SAPPeer::handleConnected()
+{
+	emit connected();
+
+	// Call in all the agents
+	SAPManager *manager = SAPManager::instance();
+	foreach (SAPAgent *agent, manager->allAgents()) {
+		agent->peerFound(this);
+	}
+}
+
+void SAPPeer::handleDisconnected()
+{
+	// Clear out all active sessions
+	emit disconnected();
+	qDeleteAll(_conns);
+	// Deleting connections will clean up all sockets too.
+	_sessions.clear();
+	_conns.clear();
+	// TODO Figure out who should actually reconnect
+}
+
+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" << s.sessionId << "is already active";
+				ok = false;
+			}
+		}
+
+		if (!ok) {
+			// Send a negative status code message back
+			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);
+			}
+
+			writeDataToSession(SAProtocol::defaultSessionId,
+							   SAProtocol::packServiceConnectionResponseFrame(resp));
+			return;
+		}
+
+		SAPConnection *conn = new SAPConnection(this, req.profile);
+
+		foreach (const SAProtocol::ServiceConnectionRequestSession &s, req.sessions) {
+			SAPChannelInfo cInfo;
+			cInfo.setChannelId(s.channelId);
+			cInfo.setQoSType(static_cast<SAPChannelInfo::QoSType>(s.qosType));
+			cInfo.setQoSDataRate(static_cast<SAPChannelInfo::QoSDataRate>(s.qosDataRate));
+			cInfo.setQoSPriority(static_cast<SAPChannelInfo::QoSPriority>(s.qosPriority));
+			cInfo.setPayloadType(static_cast<SAPChannelInfo::PayloadType>(s.payloadType));
+
+			SAPSocket *socket = new SAPSocket(conn, s.sessionId, cInfo);
+			conn->setSocket(s.channelId, socket);
+
+			qDebug() << " opening channel" << s.channelId << "as session" << s.sessionId;
+			qDebug() << " " << cInfo;
+
+			_sessions.insert(s.sessionId, socket);
+		}
+
+		_conns.insert(req.profile, conn);
+
+		SAPConnectionRequest *connReq = new SAPConnectionRequest(conn, req.initiatorId, req.acceptorId);
+		agent->requestConnection(connReq);
+
+		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();
+
+		if (ok) {
+			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;
+				}
+			}
+
+			emit conn->connected();
+		} else {
+			emit conn->error(frame.statusCode);
+			emit conn->disconnected();
+			delete conn;
+		}
+
+		break;
+	}
+	case SAProtocol::ServiceTerminationRequest: {
+		SAProtocol::ServiceTerminationRequestFrame req = SAProtocol::unpackServiceTerminationRequestFrame(message);
+		SAPConnection *conn = _conns.value(req.profile, 0);
+
+		qDebug() << "Service termination request to profile" << req.profile;
+
+		if (!conn) {
+			// We did not find this profile; send a error back
+			SAProtocol::ServiceTerminationResponseFrame resp;
+			resp.messageType = SAProtocol::ServiceTerminationResponse;
+			resp.acceptorId = req.acceptorId;
+			resp.initiatorId = req.initiatorId;
+			resp.profile = req.profile;
+			resp.statusCode = 1;
+
+			qWarning() << "Profile" << req.profile << "not connected, sending negative response";
+
+			writeDataToSession(SAProtocol::defaultSessionId,
+							   SAProtocol::packServiceTerminationResponseFrame(resp));
+			return;
+		}
+
+		// Ok, proceed to terminate the connection
+		foreach (SAPSocket *socket, conn->sockets()) {
+			emit socket->disconnected();
+			_sessions.remove(socket->sessionId());
+		}
+		emit conn->disconnected();
+		_conns.remove(conn->profile());
+		delete conn;
+
+		// Acknowledge everything was succesful
+		SAProtocol::ServiceTerminationResponseFrame resp;
+		resp.messageType = SAProtocol::ServiceTerminationResponse;
+		resp.acceptorId = req.acceptorId;
+		resp.initiatorId = req.initiatorId;
+		resp.profile = req.profile;
+		resp.statusCode = 0;
+
+		writeDataToSession(SAProtocol::defaultSessionId,
+						   SAProtocol::packServiceTerminationResponseFrame(resp));
+
+		break;
+	}
+	default:
+		qWarning() << "Unknown default session message type:" << message_type;
+	}
+}
diff --git a/sap/sappeer.h b/sap/sappeer.h
new file mode 100644
index 0000000..1c477c4
--- /dev/null
+++ b/sap/sappeer.h
@@ -0,0 +1,77 @@
+#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);
+	~SAPPeer();
+
+	SAPConnection* createServiceConnection(const QString &profile, const QString &requesterProfile, SAPServiceInfo::Role requesterRole);
+	bool terminateServiceConnection(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. */
+	void writeDataToSession(int session, const QByteArray &data);
+	/** Writes a control frame to the remote. */
+	void writeControlToSession(int session, const QByteArray &control);
+
+	void acceptServiceConnection(SAPConnectionRequest *connReq, int statusCode);
+
+	/** Distributes data to the appropiate socket. */
+	void handleSessionData(int session, const QByteArray &data);
+
+	/** Distributes a control message to the apropiate socket. */
+	void handleSessionControl(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<QString, SAPConnection*> _conns;
+	QMap<int, SAPSocket*> _sessions;
+
+	friend class SAPSocket;
+	friend class SAPConnectionRequest;
+};
+
+#endif // SAPPEER_H
diff --git a/sap/saprotocol.cc b/sap/saprotocol.cc
new file mode 100644
index 0000000..b27c01e
--- /dev/null
+++ b/sap/saprotocol.cc
@@ -0,0 +1,651 @@
+#include <QtCore/QDebug>
+#include "endianhelpers.h"
+#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");
+
+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, FrameType type)
+{
+	Frame frame;
+	frame.protocolVersion = currentProtocolVersion;
+	frame.type = type;
+	frame.sessionId = sessionId;
+	frame.data = data;
+	return packFrame(frame);
+}
+
+SAProtocol::DataFrame SAProtocol::unpackDataFrame(const QByteArray &data, bool withSeqNum, bool withFragStatus)
+{
+	DataFrame frame;
+	int offset = 0;
+	frame.withSeqNum = withSeqNum;
+	frame.withFragStatus = withFragStatus;
+	if (withSeqNum) {
+		frame.seqNum = read<quint16>(data, offset);
+	}
+	if (withFragStatus) {
+		frame.fragStatus = static_cast<SAProtocol::FragmentStatus>(read<quint8>(data, offset));
+	}
+	frame.data = data.mid(offset);
+	return frame;
+}
+
+QByteArray SAProtocol::packDataFrame(const DataFrame &frame)
+{
+	QByteArray data;
+	if (frame.withSeqNum) {
+		append<quint16>(data, frame.seqNum);
+	}
+	if (frame.withFragStatus) {
+		append<quint8>(data, frame.fragStatus);
+	}
+	data.append(frame.data);
+	return data;
+}
+
+SAProtocol::ControlFrame SAProtocol::unpackControlFrame(const QByteArray &data)
+{
+	ControlFrame frame;
+	int offset = 0;
+	int num_ranges;
+
+	frame.type = static_cast<ControlFrameType>(read<quint8>(data, offset));
+
+	switch (frame.type) {
+	case ControlFrameImmediateAck:
+	case ControlFrameBlockAck:
+		frame.seqNum = read<quint16>(data, offset);
+		break;
+	case ControlFrameNak:
+		num_ranges = read<quint8>(data, offset);
+		frame.seqNums.reserve(num_ranges);
+		for (int i = 0; i < num_ranges; i++) {
+			SeqNumRange r;
+			r.first = read<quint8>(data, offset);
+			r.second = read<quint8>(data, offset);
+			frame.seqNums.append(r);
+		}
+		break;
+	default:
+		qWarning() << "Unknown frame type";
+		break;
+	}
+
+	return frame;
+}
+
+QByteArray SAProtocol::packControlFrame(const ControlFrame &frame)
+{
+	QByteArray data;
+	append<quint8>(data, frame.type);
+	switch (frame.type) {
+	case ControlFrameImmediateAck:
+	case ControlFrameBlockAck:
+		Q_ASSERT(frame.seqNums.empty());
+		append<quint16>(data, frame.seqNum);
+		break;
+	case ControlFrameNak:
+		append<quint8>(data, frame.seqNums.size());
+		foreach (const SeqNumRange &r, frame.seqNums) {
+			append<quint16>(data, r.first);
+			append<quint16>(data, r.second);
+		}
+		break;
+	}
+	return data;
+}
+
+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;
+
+	const int num_sessions = read<quint16>(data, offset);
+	frame.sessions.reserve(num_sessions);
+
+	for (int i = 0; i < num_sessions; i++) {
+		ServiceConnectionRequestSession session;
+		session.sessionId = read<quint16>(data, offset);
+		frame.sessions.append(session);
+	}
+
+	for (int i = 0; i < num_sessions; i++) {
+		ServiceConnectionRequestSession &session = frame.sessions[i];
+		session.channelId = read<quint16>(data, offset);
+	}
+
+	for (int i = 0; i < num_sessions; i++) {
+		ServiceConnectionRequestSession &session = frame.sessions[i];
+		session.qosType = read<quint8>(data, offset);
+		session.qosDataRate = read<quint8>(data, offset);
+		session.qosPriority = read<quint8>(data, offset);
+	}
+
+	for (int i = 0; i < num_sessions; i++) {
+		ServiceConnectionRequestSession &session = frame.sessions[i];
+		session.payloadType = read<quint8>(data, offset);
+	}
+
+	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);
+	}
+
+	foreach (const ServiceConnectionRequestSession &session, frame.sessions) {
+		append<quint16>(data, session.channelId);
+	}
+
+	foreach (const ServiceConnectionRequestSession &session, frame.sessions) {
+		append<quint8>(data, session.qosType);
+		append<quint8>(data, session.qosDataRate);
+		append<quint8>(data, session.qosPriority);
+	}
+
+	foreach (const ServiceConnectionRequestSession &session, frame.sessions) {
+		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<quint16>(data, offset);
+	frame.initiatorId = read<quint16>(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::ServiceTerminationRequestFrame SAProtocol::unpackServiceTerminationRequestFrame(const QByteArray &data)
+{
+	ServiceTerminationRequestFrame 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 termination request";
+		return frame;
+	}
+
+	frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
+
+	return frame;
+}
+
+QByteArray SAProtocol::packServiceTerminationRequestFrame(const ServiceTerminationRequestFrame &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(';');
+
+	return data;
+}
+
+SAProtocol::ServiceTerminationResponseFrame SAProtocol::unpackServiceTerminationResponseFrame(const QByteArray &data)
+{
+	ServiceTerminationResponseFrame 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 termination response";
+		return frame;
+	}
+
+	frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset);
+	offset = marker + 1;
+
+	frame.statusCode = read<quint8>(data, offset);
+
+	return frame;
+}
+
+QByteArray SAProtocol::packServiceTerminationResponseFrame(const ServiceTerminationResponseFrame &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);
+
+	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/sap/saprotocol.h b/sap/saprotocol.h
new file mode 100644
index 0000000..68434c1
--- /dev/null
+++ b/sap/saprotocol.h
@@ -0,0 +1,246 @@
+#ifndef SAPROTOCOL_H
+#define SAPROTOCOL_H
+
+#include <QtCore/QObject>
+#include <QtBluetooth/QBluetoothUuid>
+
+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; // Always 0 so far.
+		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, FrameType type = FrameData);
+
+	enum FragmentStatus {
+		FragmentNone = 0,
+		FragmentMiddle = 1,
+		FragmentLast = 3
+	};
+
+	struct DataFrame {
+		bool withSeqNum; // (not actually present in frame)
+		// The following field is only present if "withSeqNum":
+		quint16 seqNum; // Monotonically increasing
+		bool withFragStatus; // (not actually present in frame)
+		FragmentStatus fragStatus;
+		QByteArray data;
+	};
+
+	static DataFrame unpackDataFrame(const QByteArray &data, bool withSeqNum, bool withFragStatus);
+	static QByteArray packDataFrame(const DataFrame& frame);
+
+	enum ControlFrameType {
+		ControlFrameImmediateAck = 0,
+		ControlFrameBlockAck = 1,
+		ControlFrameNak = 2
+	};
+
+	typedef QPair<quint16, quint16> SeqNumRange;
+
+	struct ControlFrame {
+		ControlFrameType type;
+		QList<SeqNumRange> seqNums; // Used for Naks only
+		quint16 seqNum;
+	};
+
+	static ControlFrame unpackControlFrame(const QByteArray &data);
+	static QByteArray packControlFrame(const ControlFrame& frame);
+
+	/* Default session messages */
+
+	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);
+
+	struct ServiceTerminationRequestFrame {
+		quint8 messageType;
+		quint16 acceptorId;
+		quint16 initiatorId;
+		QString profile;
+	};
+
+	static ServiceTerminationRequestFrame unpackServiceTerminationRequestFrame(const QByteArray &data);
+	static QByteArray packServiceTerminationRequestFrame(const ServiceTerminationRequestFrame &frame);
+
+	struct ServiceTerminationResponseFrame {
+		quint8 messageType;
+		quint16 acceptorId;
+		quint16 initiatorId;
+		QString profile;
+		quint8 statusCode;
+	};
+
+	static ServiceTerminationResponseFrame unpackServiceTerminationResponseFrame(const QByteArray &data);
+	static QByteArray packServiceTerminationResponseFrame(const ServiceTerminationResponseFrame &frame);
+
+	/* Capability discovery messages */
+
+	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/sap/sapserviceinfo.cc b/sap/sapserviceinfo.cc
new file mode 100644
index 0000000..a77d2f0
--- /dev/null
+++ b/sap/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/sap/sapserviceinfo.h b/sap/sapserviceinfo.h
new file mode 100644
index 0000000..91756b7
--- /dev/null
+++ b/sap/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/sap/sapsocket.cc b/sap/sapsocket.cc
new file mode 100644
index 0000000..390aef0
--- /dev/null
+++ b/sap/sapsocket.cc
@@ -0,0 +1,250 @@
+#include <QtCore/QDebug>
+#include <QtCore/QTimerEvent>
+#include <limits>
+
+#include "sappeer.h"
+#include "sapconnection.h"
+#include "sapsocket.h"
+
+#define DELAYED_ACK_TIME 1000
+#define RESEND_TIME 5000
+#define WINDOW_SIZE_MSGS 10
+
+SAPSocket::SAPSocket(SAPConnection *conn, int sessionId, const SAPChannelInfo &chanInfo) :
+	QObject(conn), _sessionId(sessionId), _info(chanInfo), _open(false),
+	_outLastAck(0), _outFlyingPkts(0), _inLastSeqNum(0), _inLastAck(0)
+{
+}
+
+SAPPeer * SAPSocket::peer()
+{
+	return connection()->peer();
+}
+
+SAPConnection * SAPSocket::connection()
+{
+	return static_cast<SAPConnection*>(parent());
+}
+
+SAPChannelInfo SAPSocket::channelInfo() const
+{
+	return _info;
+}
+
+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)
+{
+	if (!isOpen()) {
+		qWarning() << "Socket is not yet open";
+		return false;
+	}
+
+	if (data.size() > 65000) {
+		qWarning() << "Fragmentation is not yet supported";
+		return false;
+	}
+
+	if (isReliable()) {
+		int seqNum = (_outLastAck + _out.size() + 1) % std::numeric_limits<quint16>::max();
+		if (_outFlyingPkts >= WINDOW_SIZE_MSGS) {
+			// Send buffer is not empty; enqueue
+			qDebug() << _sessionId << "Enqueuing" << seqNum << " size=" << data.size();
+		} else {
+			qDebug() << _sessionId << "Sending" << seqNum << " size=" << data.size();
+			sendPacket(seqNum, data);
+			_outFlyingPkts++;
+		}
+		_out.append(data);
+		if (!_resendTimer.isActive()) {
+			_resendTimer.start(RESEND_TIME, Qt::CoarseTimer, this);
+		}
+		qDebug() << _sessionId << "pending" << _out.size() << " flying" << _outFlyingPkts;
+	} else {
+		// Just send the packet as is.
+		sendPacket(0, data);
+	}
+
+	return true;
+}
+
+void SAPSocket::setOpen(bool open)
+{
+	_open = open;
+}
+
+void SAPSocket::acceptIncomingData(const QByteArray &data)
+{
+	SAProtocol::DataFrame frame = SAProtocol::unpackDataFrame(data,
+									isReliable(), supportsFragmentation());
+
+	if (isReliable()) {
+		quint16 expectedSeqNum = _inLastSeqNum + 1;
+		if (frame.seqNum != expectedSeqNum) {
+			qWarning() << "Unexpected sequence number" << frame.seqNum
+					   << "on session" << _sessionId
+					   << "(expected " << expectedSeqNum << ")";
+		} else {
+			_inLastSeqNum = frame.seqNum;
+			qDebug() << _sessionId << "Got seqNum" << frame.seqNum << " size=" << data.size();
+
+			if (!_ackTimer.isActive()) {
+				_ackTimer.start(DELAYED_ACK_TIME, Qt::CoarseTimer, this);
+			}
+		}
+	}
+
+	_in.enqueue(frame.data);
+
+	emit messageReceived();
+}
+
+void SAPSocket::acceptIncomingControl(const QByteArray &data)
+{
+	SAProtocol::ControlFrame frame = SAProtocol::unpackControlFrame(data);
+
+	if (!isReliable()) {
+		qWarning() << "Received a control frame but socket is not in QoS mode";
+		return;
+	}
+
+	switch (frame.type) {
+	case SAProtocol::ControlFrameBlockAck:
+		handleBlockAck(frame.seqNum);
+		break;
+	default:
+		qWarning() << "Unhandled control frame type" << frame.type;
+		break;
+	}
+}
+
+int SAPSocket::sessionId() const
+{
+	return _sessionId;
+}
+
+void SAPSocket::timerEvent(QTimerEvent *event)
+{
+	if (event->timerId() == _ackTimer.timerId()) {
+		qDebug() << "Ack timer tick";
+		if (_inLastSeqNum != _inLastAck) {
+			sendBlockAck(_inLastSeqNum);
+
+			_inLastAck = _inLastSeqNum;
+		}
+		_ackTimer.stop();
+	} else if (event->timerId() == _resendTimer.timerId()) {
+		qDebug() << "Resend timer tick";
+		if (!_out.isEmpty()) {
+			_outFlyingPkts = 0; // Assume everything went wrong
+			sendPacketsFromQueue();
+		} else {
+			_resendTimer.stop();
+		}
+	} else {
+		QObject::timerEvent(event);
+	}
+}
+
+bool SAPSocket::isReliable() const
+{
+	return _info.qosType() == SAPChannelInfo::QoSReliabilityEnable;
+}
+
+bool SAPSocket::supportsFragmentation() const
+{
+	return _info.qosType() == SAPChannelInfo::QoSReliabilityEnable ||
+		   _info.qosType() == SAPChannelInfo::QoSReliabilityDisable;
+}
+
+void SAPSocket::sendBlockAck(int seqNum)
+{
+	SAProtocol::ControlFrame frame;
+	frame.type = SAProtocol::ControlFrameBlockAck;
+	frame.seqNum = seqNum;
+	peer()->writeControlToSession(_sessionId, SAProtocol::packControlFrame(frame));
+}
+
+void SAPSocket::sendPacket(int seqNum, const QByteArray &data)
+{
+	SAProtocol::DataFrame frame;
+	frame.withSeqNum = isReliable();
+	frame.seqNum = seqNum;
+	frame.withFragStatus = supportsFragmentation();
+	frame.fragStatus = SAProtocol::FragmentNone;
+	frame.data = data;
+	peer()->writeDataToSession(_sessionId, SAProtocol::packDataFrame(frame));
+}
+
+void SAPSocket::handleBlockAck(int seqNum)
+{
+	qDebug() << _sessionId << "Received block ack for" << seqNum;
+	int n = seqNum - _outLastAck;
+	if (n < 0) n += std::numeric_limits<quint16>::max();
+
+	qDebug() << _sessionId << "pending" << _out.size() << " flying" << _outFlyingPkts << " n" << n;
+
+	if (n <= 0) {
+		qWarning() << _sessionId << "repeated ack";
+		return;
+	}
+
+	if (n <= _out.size()) {
+		_out.erase(_out.begin(), _out.begin() + n);
+		_outLastAck += static_cast<unsigned int>(n);
+		_outFlyingPkts -= n;
+		qDebug() << _sessionId << "new outlastack" << _outLastAck;
+	}
+
+	qDebug() << _sessionId << "pending" << _out.size() << " flying" << _outFlyingPkts;
+
+	Q_ASSERT(_outFlyingPkts <= _out.size());
+
+	if (_outFlyingPkts == 0 || _out.size() == 0) {
+		qDebug() << _sessionId << "Stopping resend timer";
+		_resendTimer.stop();
+	}
+
+	sendPacketsFromQueue();
+}
+
+void SAPSocket::sendPacketsFromQueue()
+{
+	const int n = qMin(WINDOW_SIZE_MSGS, _out.size()) - _outFlyingPkts;
+
+	Q_ASSERT(n >= 0);
+
+	for (int i = _outFlyingPkts; i < _outFlyingPkts + n; i++) {
+		int seqNum = _outLastAck + i + 1;
+		const QByteArray &pkt = _out.at(i);
+		qDebug() << _sessionId << "Sending packet" << seqNum << "size=" << pkt.size();
+		sendPacket(seqNum, pkt);
+	}
+
+	_outFlyingPkts += n;
+
+	Q_ASSERT(_outFlyingPkts <= _out.size());
+
+	qDebug() << _sessionId << "pending" << _out.size() << " flying" << _outFlyingPkts << " n" << n;
+
+	if (n > 0 && !_resendTimer.isActive()) {
+		_resendTimer.start(RESEND_TIME, Qt::CoarseTimer, this);
+	}
+}
diff --git a/sap/sapsocket.h b/sap/sapsocket.h
new file mode 100644
index 0000000..59fcb3c
--- /dev/null
+++ b/sap/sapsocket.h
@@ -0,0 +1,76 @@
+#ifndef SAPSOCKET_H
+#define SAPSOCKET_H
+
+#include <QtCore/QObject>
+#include <QtCore/QQueue>
+#include <QtCore/QBasicTimer>
+
+#include "sapchannelinfo.h"
+
+class SAPConnection;
+class SAPPeer;
+
+class SAPSocket : public QObject
+{
+	Q_OBJECT
+
+	SAPSocket(SAPConnection *conn, int sessionId, const SAPChannelInfo &chanInfo);
+
+public:
+	SAPPeer *peer();
+	SAPConnection *connection();
+
+	SAPChannelInfo channelInfo() const;
+
+	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);
+	void acceptIncomingControl(const QByteArray &data);
+
+	int sessionId() const;
+
+	virtual void timerEvent(QTimerEvent *event) override;
+
+private:
+	bool isReliable() const;
+	bool supportsFragmentation() const;
+
+	void sendBlockAck(int seqNum);
+	void sendPacket(int seqNum, const QByteArray &data);
+
+	void handleBlockAck(int seqNum);
+	void sendPacketsFromQueue();
+
+private:
+	const int _sessionId;
+	const SAPChannelInfo _info;
+	bool _open;
+	QQueue<QByteArray> _in;
+	QQueue<QByteArray> _out;
+	QBasicTimer _ackTimer;
+	QBasicTimer _resendTimer;
+
+	/** Last acknowledged sent message. */
+	quint16 _outLastAck;
+	quint16 _outFlyingPkts;
+
+	/** Next expected incoming sequence number */
+	quint16 _inLastSeqNum;
+	/** Last acknowledged sequence number */
+	quint16 _inLastAck;
+
+	friend class SAPPeer;
+};
+
+#endif // SAPSOCKET_H
diff --git a/sap/wmscrypt.h b/sap/wmscrypt.h
new file mode 100644
index 0000000..c7271cb
--- /dev/null
+++ b/sap/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/sap/wmskeys.h b/sap/wmskeys.h
new file mode 100644
index 0000000..5170ee8
--- /dev/null
+++ b/sap/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/sap/wmspeer.h b/sap/wmspeer.h
new file mode 100644
index 0000000..446e747
--- /dev/null
+++ b/sap/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