From cc1af30276d3650dd71589cd1d7c005da3a64873 Mon Sep 17 00:00:00 2001 From: "Javier S. Pedro" Date: Sat, 20 Sep 2014 19:18:00 +0200 Subject: add normal/rfcomm bluetooth transport for older MWs --- src/metawatchbttransport.cpp | 241 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 241 insertions(+) create mode 100644 src/metawatchbttransport.cpp (limited to 'src/metawatchbttransport.cpp') diff --git a/src/metawatchbttransport.cpp b/src/metawatchbttransport.cpp new file mode 100644 index 0000000..63996e9 --- /dev/null +++ b/src/metawatchbttransport.cpp @@ -0,0 +1,241 @@ +#include + +#include "metawatchbttransport.h" + +namespace +{ + +const quint8 bit_rev_table[16] = { + 0, 8, 4, 12, 2, 10, 6, 14, 1, 9, 5, 13, 3, 11, 7, 15 +}; + +const quint16 crc_table[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, + 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, + 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, + 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, + 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, + 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, + 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, + 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, + 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, + 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, + 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, + 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, + 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, + 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, + 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, + 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, + 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, + 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, + 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, + 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, + 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, + 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 +}; + +#if 0 /* This snippet was used to build the table seen above. */ + quint16 remainder; + int dividend; + quint8 bit; + + for (dividend = 0; dividend < 256; dividend++) { + remainder = dividend << 8; + for (bit = 8; bit > 0; bit--) { + if (remainder & 0x8000) { + remainder = (remainder << 1) ^ 0x1021; + } else { + remainder = (remainder << 1); + } + } + if ((dividend % 8) == 0) { + printf(",\n0x%04hx", remainder); + } else { + printf(", 0x%04hx", remainder); + } + } +#endif + +quint16 compute_crc(const QByteArray &data) +{ + const int size = data.size(); + quint16 remainder = 0xFFFF; + + for (int i = 0; i < size; i++) { + quint8 byte = data[i]; + byte = (bit_rev_table[byte & 0xF] << 4) | bit_rev_table[(byte & 0xF0) >> 4]; + remainder = crc_table[byte ^ (remainder >> 8)] ^ (remainder << 8); + } + + return remainder; +} + +} + +MetaWatchBTTransport::MetaWatchBTTransport(const QBluetoothAddress &address, QObject *parent) : + MetaWatchTransport(parent), _addr(address), _socket(0) +{ + +} + +MetaWatchBTTransport::~MetaWatchBTTransport() +{ + disconnectDevice(); +} + +bool MetaWatchBTTransport::isDeviceConnected() const +{ + return _socket && _socket->state() == QBluetoothSocket::ConnectedState; +} + +void MetaWatchBTTransport::sendMessage(quint8 type, quint8 options, const QByteArray &payload) +{ + QByteArray packet = encode(type, options, payload); + qDebug() << "Send:" << packet.toHex(); + qint64 written = _socket->write(packet); + if (written < packet.size()) { + qWarning() << "Packet not fully writen"; + } +} + +void MetaWatchBTTransport::connectDevice() +{ + qDebug() << "Trying to connect to" << _addr.toString(); + + if (_socket) { + disconnectDevice(); + } + + // Sadly, seems that QBluetoothSocket doesn't really like been closed and reopened, + // so we recreate it every time. + _socket = new QBluetoothSocket(QBluetoothSocket::RfcommSocket, this); + connect(_socket, &QBluetoothSocket::connected, this, &MetaWatchBTTransport::handleSocketConnected); + connect(_socket, &QBluetoothSocket::disconnected, this, &MetaWatchBTTransport::handleSocketDisconnected); + connect(_socket, &QBluetoothSocket::readyRead, this, &MetaWatchBTTransport::handleSocketData); + + // Let's hardcode MetaWatch's RFCOMM port (1) in order to avoid having + // to look it up (which would need an SDP query). + _socket->connectToService(_addr, 1); +} + +void MetaWatchBTTransport::disconnectDevice() +{ + if (_socket) { + _socket->close(); + _socket->deleteLater(); + disconnect(_socket, 0, this, 0); + _socket = 0; + } +} + +QByteArray MetaWatchBTTransport::encode(quint8 type, quint8 options, const QByteArray &payload) +{ + QByteArray packet; + + quint8 message_size = 6 + payload.size(); + packet.reserve(message_size); + + packet.append(0x01); + packet.append(message_size); + packet.append(type); + packet.append(options); + + packet.append(payload); + + packet.append("\0\0", 2); + + quint16 crc = compute_crc(packet); // Replace CRC with computed CRC + packet[message_size - 2] = crc & 0xFF; + packet[message_size - 1] = crc >> 8; + + return packet; +} + +bool MetaWatchBTTransport::decode(const QByteArray &packet, quint8 *type, quint8 *options, QByteArray *payload) +{ + if (packet.size() < 6) { + qWarning() << "Message too short"; + } + + if (packet.at(0) != 0x1) { + qWarning() << "Invalid packet header"; + return false; + } + + quint8 message_size = packet[1]; + if (message_size < 6 || message_size > 32 || message_size != packet.size()) { + qWarning() << "Invalid message size:" << message_size; + } + + *type = packet[2]; + *options = packet[3]; + + *payload = packet.mid(4, message_size - 6); + + // Don't compare received CRC... what for? RFCOMM already guarantees reliability. + + return true; +} + +void MetaWatchBTTransport::handleSocketConnected() +{ + qDebug() << "MW connected"; + emit connected(); +} + +void MetaWatchBTTransport::handleSocketDisconnected() +{ + qDebug() << "MW disconnected"; + emit disconnected(); +} + +void MetaWatchBTTransport::handleSocketData() +{ + const int min_message_size = 6; + while (_socket->bytesAvailable() >= min_message_size) { + char header[2]; + qint64 bytes_read = _socket->peek(header, sizeof(header)); + if (bytes_read < 2) { + qWarning() << "Unable to peek received data"; + break; + } + + if (header[0] != 0x01 || header[1] > 32) { + qWarning() << "Received garbage data"; + _socket->readAll(); // let's discard all pending data. + break; + } + if (_socket->bytesAvailable() < header[1]) { + // Not enough data to complete the entire frame + break; + } + + QByteArray data = _socket->read(header[1]); + if (data.size() != header[1]) { + qWarning() << "Received incomplete packet"; + _socket->readAll(); // discard all other pending data. + break; + } + + qDebug() << "Recv:" << data.toHex(); + + quint8 type, options; + QByteArray payload; + + if (decode(data, &type, &options, &payload)) { + emit messageReceived(type, options, payload); + } else { + qWarning() << "Failed to decode message from metawatch:" << data.toHex(); + } + } +} -- cgit v1.2.3