1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
|
#include <zlib.h>
#include <QtDebug>
#include <QtEndian>
#include <QtCore/QMetaEnum>
#include "toqconnection.h"
static const int HEADER_LENGTH = 10;
ToqConnection::ToqConnection(const QBluetoothAddress &address, QObject *parent) :
QObject(parent),
_address(address), _socket(0),
_reconnectTimer(new QTimer(this)),
_lastTransactionId(0)
{
connect(_reconnectTimer, &QTimer::timeout,
this, &ToqConnection::tryConnect);
_reconnectTimer->setSingleShot(true);
_reconnectTimer->setInterval(1000);
_reconnectTimer->start();
}
QString ToqConnection::nameOfEndpoint(Endpoint ep)
{
int index = staticMetaObject.indexOfEnumerator("CoreEndpoints");
QMetaEnum epEnum = staticMetaObject.enumerator(index);
const char * ret = epEnum.valueToKey(ep);
if (ret) {
return QString::fromLatin1(ret);
} else {
return QString::number(ep);
}
}
quint32 ToqConnection::checksum(const QByteArray &data)
{
uLong crc = crc32(0L, Z_NULL, 0);
crc = crc32(crc, reinterpret_cast<const Bytef*>(data.constData()), data.size());
return crc;
}
quint32 ToqConnection::checksum(QIODevice *dev)
{
uLong crc = crc32(0L, Z_NULL, 0);
char buffer[4 * 1024];
qint64 read;
while ((read = dev->read(buffer, sizeof(buffer))) > 0) {
crc = crc32(crc, reinterpret_cast<const Bytef*>(&buffer[0]), read);
}
return crc;
}
quint16 ToqConnection::newTransactionId()
{
if (_lastTransactionId >= 0xFFFA) {
// The last transaction ids (as well as 0) seem to be reserved
// Avoid using them
_lastTransactionId = 0;
}
return ++_lastTransactionId;
}
void ToqConnection::sendMessage(const Message &msg)
{
if (_socket) {
_socket->write(packMessage(msg));
} else {
qWarning() << "Discarding message because connection is broken";
}
}
ToqConnection::Message ToqConnection::unpackMessage(const QByteArray &data)
{
Message msg;
Q_ASSERT(data.length() >= HEADER_LENGTH);
const uchar *header = reinterpret_cast<const uchar*>(data.constData());
quint16 message_length = qFromBigEndian<quint16>(&header[2]);
Q_ASSERT(data.length() == message_length + HEADER_LENGTH - 4);
msg.source = header[0];
msg.destination = header[1];
msg.transactionId = qFromBigEndian<quint16>(&header[4]);
msg.type = qFromBigEndian<quint32>(&header[6]);
if (!data.isEmpty()) {
QJsonParseError error;
msg.payload = QJsonDocument::fromJson(data.mid(HEADER_LENGTH), &error);
if (error.error) {
qWarning() << "Failure while parsing message JSON payload: " << error.errorString();
}
}
return msg;
}
QByteArray ToqConnection::packMessage(const Message &msg)
{
QByteArray payload = msg.payload.toJson(QJsonDocument::Compact);
uchar header[HEADER_LENGTH];
header[0] = msg.source;
header[1] = msg.destination;
qToBigEndian<quint16>(payload.length() + 4, &header[2]);
qToBigEndian<quint16>(msg.transactionId, &header[4]);
qToBigEndian<quint32>(msg.type, &header[6]);
payload.prepend(reinterpret_cast<char*>(&header[0]), HEADER_LENGTH);
return payload;
}
void ToqConnection::tryConnect()
{
Q_ASSERT(!_socket);
_socket = new QBluetoothSocket(QBluetoothServiceInfo::RfcommProtocol, this);
connect(_socket, &QBluetoothSocket::connected,
this, &ToqConnection::handleSocketConnected);
connect(_socket, &QBluetoothSocket::disconnected,
this, &ToqConnection::handleSocketDisconnected);
connect(_socket, (void (QBluetoothSocket::*)(QBluetoothSocket::SocketError))&QBluetoothSocket::error,
this, &ToqConnection::handleSocketError);
connect(_socket, &QBluetoothSocket::readyRead,
this, &ToqConnection::handleSocketData);
qDebug() << "Connecting to" << _address.toString();
_socket->connectToService(_address, 1);
}
void ToqConnection::handleSocketConnected()
{
qDebug() << "Connected";
Q_ASSERT(_socket);
_reconnectTimer->stop();
emit connected();
emit connectedChanged();
}
void ToqConnection::handleSocketDisconnected()
{
if (_socket) {
qDebug() << "Disconnected";
_socket->deleteLater();
_socket = 0;
_reconnectTimer->start();
emit disconnected();
emit connectedChanged();
}
}
void ToqConnection::handleSocketError(QBluetoothSocket::SocketError error)
{
if (_socket) {
qWarning() << error << _socket->errorString();
_socket->disconnectFromService();
}
}
void ToqConnection::handleSocketData()
{
// Keep attempting to read messages as long as at least a header is present
while (_socket->bytesAvailable() >= HEADER_LENGTH) {
// Take a look at the header, but do not remove it from the socket input buffer.
// We will only remove it once we're sure the entire packet is in the buffer.
uchar header[HEADER_LENGTH];
_socket->peek(reinterpret_cast<char*>(header), HEADER_LENGTH);
quint16 message_length = qFromBigEndian<quint16>(&header[2]);
// Sanity checks on the message_length
if (message_length == 0) {
qWarning() << "received empty message";
_socket->read(HEADER_LENGTH); // skip this header
continue; // check if there are additional headers.
}
// Now wait for the entire message
if (_socket->bytesAvailable() < HEADER_LENGTH + message_length - 4) {
qDebug() << "incomplete msg body in read buffer";
return; // try again once more data comes in
}
// We can now safely remove the message from the input buffer,
// as we know the entire message is in the input buffer.
QByteArray data = _socket->read(HEADER_LENGTH + message_length - 4);
Message msg = unpackMessage(data);
if (msg.transactionId > _lastTransactionId) _lastTransactionId = msg.transactionId;
emit messageReceived(msg);
}
}
|
