properly handle data frames with sequence numbers

14 files changed, 139 insertions, 37 deletions

diff --git a/hostmanagerconn.cc b/hostmanagerconn.cc
index e4f5edc..b8d1d31 100644
--- a/hostmanagerconn.cc
+++ b/hostmanagerconn.cc
@@ -26,7 +26,7 @@ void HostManagerConn::sendMessage(const QJsonObject &msg)
 	QJsonDocument doc(msg);
 	QByteArray data = doc.toJson(QJsonDocument::Compact);
 	qDebug() << "Send JSON:" << data;
-	_socket->send(QByteArray(3,0) + doc.toJson(QJsonDocument::Compact));
+	_socket->send(doc.toJson(QJsonDocument::Compact));
 }
 
 void HostManagerConn::handleMessage(const QJsonObject &msg)
@@ -75,11 +75,13 @@ void HostManagerConn::performTimeSync()
 	msg["usingCamera"] = QLatin1String("false");
 	msg["safety_cam"] = QLatin1String("0");
 
-	msg["locale"] = QLocale::system().name(); // ie es_ES
-	msg["data1224"] = QLatin1String("24"); // TODO
+	QLocale l = QLocale::system();
+	msg["locale"] = l.name(); // i.e. es_ES
+	msg["data1224"] = l.timeFormat().contains('a', Qt::CaseInsensitive) ? QLatin1String("12") : QLatin1String("24");
 	msg["dateformat"] = QLocale::system().dateFormat(QLocale::ShortFormat);
 
 #if SAILFISH
+	// QTimeZone does not seem to work on Sailfish; use timed.
 	msg["timezone"] = monitor->timezone();
 #else
 	msg["timezone"] = QString::fromLatin1(QTimeZone::systemTimeZoneId());
@@ -159,13 +161,11 @@ void HostManagerConn::handleMessageReceived()
 {
 	QByteArray data = _socket->receive();
 
-	if (data.size() < 5) {
+	if (data.size() < 4) {
 		qWarning() << "Invalid HostManager message received";
 		return;
 	}
 
-	data.remove(0, 3); // What are those first three bytes??? They seem to be always NUL.
-
 	qDebug() << "Got JSON:" << QString::fromUtf8(data);
 
 	QJsonParseError error;
diff --git a/notificationconn.cc b/notificationconn.cc
index 9d596ea..592f3e5 100644
--- a/notificationconn.cc
+++ b/notificationconn.cc
@@ -104,7 +104,6 @@ QByteArray NotificationConn::packNotification(const Notification &n)
 void NotificationConn::sendNotification(const Notification &n)
 {
 	QByteArray packet = packNotification(n);
-	packet.prepend('\0');
 
 	qDebug() << packet.toHex();
 
@@ -129,7 +128,6 @@ void NotificationConn::handleConnected()
 void NotificationConn::handleMessageReceived()
 {
 	QByteArray data = _socket->receive();
-	data.remove(0, 1); // Remove first null byte
 
 	qDebug() << "Got notif msg" << data.toHex();
 
diff --git a/sapbtlistener.cc b/sapbtlistener.cc
index 2265516..cd8c015 100644
--- a/sapbtlistener.cc
+++ b/sapbtlistener.cc
@@ -131,7 +131,6 @@ void SAPBTListener::nudge(const QBluetoothAddress &address)
 		OrgBluezDeviceInterface device("org.bluez", item.path(), bus);
 		QVariantMap properties = device.GetProperties();
 		QBluetoothAddress devAddress(properties["Address"].toString());
-		qDebug() << "Found bluez device with address" << devAddress.toString();
 		if (devAddress == address) {
 			OrgBluezHeadsetInterface headset("org.bluez", item.path(), bus);
 			qDebug() << "Creating HFP connection to" << devAddress.toString();
@@ -163,7 +162,8 @@ void SAPBTListener::acceptConnection()
 	qDebug() << "Got connection";
 
 	// TODO Why am I hardcoding the role here
-	new SAPBTPeer(SAProtocol::ClientRole, socket, this);
+	SAPBTPeer *peer = new SAPBTPeer(SAProtocol::ClientRole, socket, this);
+	connect(peer, SIGNAL(disconnected()), peer, SLOT(deleteLater()));
 }
 
 void SAPBTListener::handleNudgeConnected()
diff --git a/sapbtpeer.cc b/sapbtpeer.cc
index e026a4c..3926cdf 100644
--- a/sapbtpeer.cc
+++ b/sapbtpeer.cc
@@ -27,6 +27,7 @@ void SAPBTPeer::handleSocketData()
 	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;
diff --git a/sapchannelinfo.cc b/sapchannelinfo.cc
index 12ba5b5..530341a 100644
--- a/sapchannelinfo.cc
+++ b/sapchannelinfo.cc
@@ -78,3 +78,12 @@ 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/sapchannelinfo.h b/sapchannelinfo.h
index e6db58b..922136e 100644
--- a/sapchannelinfo.h
+++ b/sapchannelinfo.h
@@ -3,6 +3,7 @@
 
 #include <QtCore/QObject>
 #include <QtCore/QSharedDataPointer>
+#include <QtCore/QDebug>
 
 class SAPChannelInfoData;
 
@@ -62,4 +63,6 @@ private:
 	QSharedDataPointer<SAPChannelInfoData> data;
 };
 
+QDebug operator<<(QDebug debug, const SAPChannelInfo &info);
+
 #endif // SAPCHANNELINFO_H
diff --git a/sapconnection.cc b/sapconnection.cc
index 970ad11..3d8ef5d 100644
--- a/sapconnection.cc
+++ b/sapconnection.cc
@@ -28,7 +28,7 @@ QList<SAPSocket*> SAPConnection::sockets()
 
 void SAPConnection::close()
 {
-	// TODO
+	// TODO implement termination of connections
 }
 
 void SAPConnection::setSocket(int channelId, SAPSocket *socket)
diff --git a/sappeer.cc b/sappeer.cc
index b9f0136..4660522 100644
--- a/sappeer.cc
+++ b/sappeer.cc
@@ -66,8 +66,7 @@ SAPConnection * SAPPeer::createServiceConnection(const QString &profile, const Q
 
 		request.sessions.append(session);
 
-		SAPSocket *socket = new SAPSocket(conn, sessionId);
-		// TODO set socket QoS parameters
+		SAPSocket *socket = new SAPSocket(conn, sessionId, cInfo);
 		conn->setSocket(session.channelId, socket);
 
 		_sessions.insert(sessionId, socket);
@@ -184,9 +183,8 @@ void SAPPeer::handleConnected()
 
 void SAPPeer::handleDisconnected()
 {
-	// TODO
+	// TODO Figure out who should actually reconnect
 	emit disconnected();
-	deleteLater();
 }
 
 int SAPPeer::findUnusedSessionId() const
@@ -253,12 +251,18 @@ void SAPPeer::handleDefaultSessionMessage(const QByteArray &message)
 		SAPConnection *conn = new SAPConnection(this, req.profile);
 
 		foreach (const SAProtocol::ServiceConnectionRequestSession &s, req.sessions) {
-			SAPSocket *socket = new SAPSocket(conn, s.sessionId);
-			// TODO set socket QoS parameters
+			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() << " payload=" << s.payloadType << "qos=" << s.qosType << "prio=" << s.qosPriority << "rate=" << s.qosDataRate;
+			qDebug() << " " << cInfo;
 
 			_sessions.insert(s.sessionId, socket);
 		}
diff --git a/saprotocol.cc b/saprotocol.cc
index 9f25613..22b3111 100644
--- a/saprotocol.cc
+++ b/saprotocol.cc
@@ -229,6 +229,30 @@ QByteArray SAProtocol::packFrame(quint16 sessionId, const QByteArray &data)
 	return packFrame(frame);
 }
 
+SAProtocol::DataFrame SAProtocol::unpackDataFrame(const QByteArray &data, bool withSeqNum)
+{
+	DataFrame frame;
+	int offset = 0;
+	frame.withSeqNum = withSeqNum;
+	if (withSeqNum) {
+		frame.seqNum = read<quint16>(data, offset);
+		frame.unk_1 = 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);
+		append<quint8>(data, frame.unk_1);
+	}
+	data.append(frame.data);
+	return data;
+}
+
 SAProtocol::ServiceConnectionRequestFrame SAProtocol::unpackServiceConnectionRequestFrame(const QByteArray &data)
 {
 	ServiceConnectionRequestFrame frame;
diff --git a/saprotocol.h b/saprotocol.h
index 443206b..79c6bcd 100644
--- a/saprotocol.h
+++ b/saprotocol.h
@@ -83,7 +83,7 @@ public:
 	};
 
 	struct Frame {
-		quint8 protocolVersion;
+		quint8 protocolVersion; // Always 0 so far.
 		FrameType type;
 		quint16 sessionId;
 		QByteArray data;
@@ -93,6 +93,20 @@ public:
 	static QByteArray packFrame(const Frame& frame);
 	static QByteArray packFrame(quint16 sessionId, const QByteArray &data);
 
+
+	struct DataFrame {
+		bool withSeqNum; // (not actually present in frame)
+		// The following fields are only present if "withSeqNum":
+		quint16 seqNum; // Monotonically increasing, but only for certain sessions...
+		quint8 unk_1; // No idea, seems to be always 0
+		QByteArray data;
+	};
+
+	static DataFrame unpackDataFrame(const QByteArray &data, bool withSeqNum);
+	static QByteArray packDataFrame(const DataFrame& frame);
+
+	/* Default session messages */
+
 	enum DefaultSessionMessageType {
 		ServiceConnectionRequest = 1,
 		ServiceConnectionResponse = 2,
@@ -156,6 +170,8 @@ public:
 	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;
diff --git a/sapsocket.cc b/sapsocket.cc
index ce85c0c..472865c 100644
--- a/sapsocket.cc
+++ b/sapsocket.cc
@@ -4,8 +4,9 @@
 #include "sapconnection.h"
 #include "sapsocket.h"
 
-SAPSocket::SAPSocket(SAPConnection *conn, int sessionId) :
-    QObject(conn), _sessionId(sessionId), _open(false)
+SAPSocket::SAPSocket(SAPConnection *conn, int sessionId, const SAPChannelInfo &chanInfo) :
+	QObject(conn), _sessionId(sessionId), _info(chanInfo), _open(false),
+	_seqNum(0), _expectedSeqNum(0)
 {
 }
 
@@ -19,6 +20,11 @@ SAPConnection * SAPSocket::connection()
 	return static_cast<SAPConnection*>(parent());
 }
 
+SAPChannelInfo SAPSocket::channelInfo() const
+{
+	return _info;
+}
+
 bool SAPSocket::isOpen() const
 {
 	return _open;
@@ -40,7 +46,18 @@ QByteArray SAPSocket::receive()
 
 bool SAPSocket::send(const QByteArray &data)
 {
-	return peer()->writeToSession(_sessionId, data);
+	SAProtocol::DataFrame frame;
+
+	frame.withSeqNum = isWithSeqNum();
+	if (isReliable()) {
+		frame.seqNum = _seqNum++;
+	} else {
+		frame.seqNum = 0;
+	}
+	frame.unk_1 = 0;
+	frame.data = data;
+
+	return peer()->writeToSession(_sessionId, SAProtocol::packDataFrame(frame));
 }
 
 void SAPSocket::setOpen(bool open)
@@ -51,7 +68,20 @@ void SAPSocket::setOpen(bool open)
 void SAPSocket::acceptIncomingData(const QByteArray &data)
 {
 	if (data.isEmpty()) return;
-	_in.enqueue(data);
+	SAProtocol::DataFrame frame = SAProtocol::unpackDataFrame(data, isWithSeqNum());
+
+	if (isReliable()) {
+		if (frame.seqNum != _expectedSeqNum) {
+			qWarning() << "Unexpected sequence number" << frame.seqNum
+					   << "on session" << _sessionId
+					   << "(expected " << _expectedSeqNum << ")";
+		}
+		_expectedSeqNum = frame.seqNum + 1;
+
+		// TODO Do we actually need to ack this somehow?
+	}
+
+	_in.enqueue(frame.data);
 
 	emit messageReceived();
 }
@@ -60,3 +90,14 @@ int SAPSocket::sessionId() const
 {
 	return _sessionId;
 }
+
+bool SAPSocket::isReliable() const
+{
+	return _info.qosType() == SAPChannelInfo::QoSReliabilityEnable;
+}
+
+bool SAPSocket::isWithSeqNum() const
+{
+	return _info.qosType() == SAPChannelInfo::QoSReliabilityDisable ||
+		   _info.qosType() == SAPChannelInfo::QoSReliabilityEnable;
+}
diff --git a/sapsocket.h b/sapsocket.h
index e1a9214..38d83ea 100644
--- a/sapsocket.h
+++ b/sapsocket.h
@@ -4,6 +4,8 @@
 #include <QtCore/QObject>
 #include <QtCore/QQueue>
 
+#include "sapchannelinfo.h"
+
 class SAPConnection;
 class SAPPeer;
 
@@ -11,12 +13,14 @@ class SAPSocket : public QObject
 {
 	Q_OBJECT
 
-	SAPSocket(SAPConnection *conn, int sessionId);
+	SAPSocket(SAPConnection *conn, int sessionId, const SAPChannelInfo &chanInfo);
 
 public:
 	SAPPeer *peer();
 	SAPConnection *connection();
 
+	SAPChannelInfo channelInfo() const;
+
 	bool isOpen() const;
 
 	bool messageAvailable() const;
@@ -35,9 +39,16 @@ protected:
 	int sessionId() const;
 
 private:
-	int _sessionId;
+	bool isReliable() const;
+	bool isWithSeqNum() const;
+
+private:
+	const int _sessionId;
+	const SAPChannelInfo _info;
 	bool _open;
 	QQueue<QByteArray> _in;
+	quint16 _seqNum;
+	quint16 _expectedSeqNum;
 
 	friend class SAPPeer;
 };
diff --git a/webproxyconn.cc b/webproxyconn.cc
index d4fe6c7..ae75e1a 100644
--- a/webproxyconn.cc
+++ b/webproxyconn.cc
@@ -5,12 +5,12 @@
 #include "webproxyconn.h"
 
 WebProxyConn::WebProxyConn(SAPConnection *conn, QObject *parent)
-	: QObject(parent), _conn(conn), _socket(conn->getSocket(501))
+	: QObject(parent), _conn(conn),
+	  _in(conn->getSocket(501)), _out(conn->getSocket(502))
 {
 	connect(_conn, SIGNAL(disconnected()), SLOT(deleteLater()));
-	Q_ASSERT(_socket);
-	connect(_socket, SIGNAL(connected()), SLOT(handleConnected()));
-	connect(_socket, SIGNAL(messageReceived()), SLOT(handleMessageReceived()));
+	Q_ASSERT(_in && _out);
+	connect(_in, SIGNAL(messageReceived()), SLOT(handleMessageReceived()));
 }
 
 WebProxyConn::RequestMessage WebProxyConn::unpackRequestMessage(const QByteArray &data)
@@ -32,14 +32,9 @@ WebProxyConn::RequestMessage WebProxyConn::unpackRequestMessage(const QByteArray
 	return msg;
 }
 
-void WebProxyConn::handleConnected()
-{
-	qDebug() << "WebProxy socket now connected!";
-}
-
 void WebProxyConn::handleMessageReceived()
 {
-	QByteArray data = _socket->receive();
+	QByteArray data = _in->receive();
 	qDebug() << data.toHex();
 	RequestMessage req = unpackRequestMessage(data);
 	qDebug() << "End of data";
diff --git a/webproxyconn.h b/webproxyconn.h
index 533feeb..264e792 100644
--- a/webproxyconn.h
+++ b/webproxyconn.h
@@ -24,12 +24,12 @@ protected:
 	static RequestMessage unpackRequestMessage(const QByteArray &data);
 
 private slots:
-	void handleConnected();
 	void handleMessageReceived();
 
 private:
 	SAPConnection *_conn;
-	SAPSocket *_socket;
+	SAPSocket *_in;
+	SAPSocket *_out;
 };
 
 #endif // WEBPROXYCONN_H