From d8d8fc7a0d139e7b864eee3b573bd208f823ad4f Mon Sep 17 00:00:00 2001 From: Javier Date: Sun, 19 Oct 2014 18:45:03 +0200 Subject: initial import, no crypto --- saprotocol.cc | 499 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 499 insertions(+) create mode 100644 saprotocol.cc (limited to 'saprotocol.cc') diff --git a/saprotocol.cc b/saprotocol.cc new file mode 100644 index 0000000..5a21762 --- /dev/null +++ b/saprotocol.cc @@ -0,0 +1,499 @@ +#include +#include +#include "saprotocol.h" + +const QBluetoothUuid SAProtocol::dataServiceUuid(QLatin1String("a49eb41e-cb06-495c-9f4f-aa80a90cdf4a")); +const QBluetoothUuid SAProtocol::nudgeServiceUuid(QLatin1String("a49eb41e-cb06-495c-9f4f-bb80a90cdf00")); + +const QLatin1String SAProtocol::capabilityDiscoveryProfile("/System/Reserved/ServiceCapabilityDiscovery"); + +namespace +{ + +template +inline T read(const QByteArray &data, int &offset) +{ + T unswapped; + qMemCopy(&unswapped, &data.constData()[offset], sizeof(T)); // Unaligned access warning! + offset += sizeof(T); + return qFromBigEndian(unswapped); +} + +template +inline void append(QByteArray &data, const T &value) +{ + T swapped = qToBigEndian(value); + data.append(reinterpret_cast(&swapped), sizeof(T)); +} + +} + +SAProtocol::SAProtocol() +{ +} + +SAProtocol::PeerDescription SAProtocol::unpackPeerDescription(const QByteArray &data) +{ + PeerDescription desc; + if (data.size() < 24) { + qWarning() << "Peer description too small"; + desc.messageType = 0; + return desc; + } + + int offset = 0; + + desc.messageType = read(data, offset); + + if (desc.messageType != 5 && desc.messageType != 6) { + qWarning() << "Unknown message type:" << desc.messageType; + return desc; + } + + desc.accessoryProtocolVersion = read(data, offset); + desc.accessorySoftwareVersion = read(data, offset); + + if (desc.messageType == 6) { + desc.status = read(data, offset); + desc.errorCode = read(data, offset); + } else { + // God knows WHYYY. + desc.status = read(data, offset); + } + desc.APDUSize = read(data, offset); + desc.SSDUSize = read(data, offset); + desc.sessions = read(data, offset); + desc.timeout = read(data, offset); + desc.unk_1 = read(data, offset); + desc.unk_2 = read(data, offset); + desc.unk_3 = read(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(data, desc.messageType); + + append(data, desc.accessoryProtocolVersion); + append(data, desc.accessorySoftwareVersion); + + if (desc.messageType == 6) { + append(data, desc.status); + append(data, desc.errorCode); + } else { + append(data, desc.status); + } + + append(data, desc.APDUSize); + append(data, desc.SSDUSize); + append(data, desc.sessions); + append(data, desc.timeout); + append(data, desc.unk_1); + append(data, desc.unk_2); + append(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(read(data, offset)); + sframe.authType = read(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(data, offset); + sframe.dataType = read(data, offset); + int len = read(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(data, sframe.type); + append(data, sframe.authType); + + switch (sframe.authType) { + case 0: + append(data, sframe.version); + append(data, sframe.dataType); + append(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((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(&header), 2); +} + +QByteArray SAProtocol::packFrame(quint16 sessionId, const QByteArray &data) +{ + Frame frame; + frame.protocolVersion = currentProtocolVersion; + frame.type = FrameData; + frame.sessionId = sessionId; + frame.data = data; + return packFrame(frame); +} + +SAProtocol::ServiceConnectionRequestFrame SAProtocol::unpackServiceConnectionRequestFrame(const QByteArray &data) +{ + ServiceConnectionRequestFrame frame; + int offset = 0; + + frame.messageType = read(data, offset); + frame.acceptorId = read(data, offset); + frame.initiatorId = read(data, offset); + + int marker = data.indexOf(';', offset); + if (marker == -1) { + qWarning() << "No profile in conn request"; + return frame; + } + + frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset); + offset = marker + 1; + + int num_sessions = read(data, offset); + + while (num_sessions > 0) { + ServiceConnectionRequestSession session; + session.sessionId = read(data, offset); + session.channelId = read(data, offset); + session.qosType = read(data, offset); + session.qosDataRate = read(data, offset); + session.qosPriority = read(data, offset); + session.payloadType = read(data, offset); + + frame.sessions.append(session); + + num_sessions--; + } + + return frame; +} + +QByteArray SAProtocol::packServiceConnectionRequestFrame(const ServiceConnectionRequestFrame &frame) +{ + QByteArray data; + + append(data, frame.messageType); + append(data, frame.acceptorId); + append(data, frame.initiatorId); + + data.append(frame.profile.toUtf8()); + data.append(';'); // Some kind of terminator + + append(data, frame.sessions.size()); + + foreach (const ServiceConnectionRequestSession &session, frame.sessions) { + append(data, session.sessionId); + append(data, session.channelId); + append(data, session.qosType); + append(data, session.qosDataRate); + append(data, session.qosPriority); + append(data, session.payloadType); + } + + return data; +} + +SAProtocol::ServiceConnectionResponseFrame SAProtocol::unpackServiceConnectionResponseFrame(const QByteArray &data) +{ + ServiceConnectionResponseFrame frame; + int offset = 0; + + frame.messageType = read(data, offset); + frame.acceptorId = read(data, offset); + frame.initiatorId = read(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(data, offset); + + int num_sessions = read(data, offset); + + while (num_sessions > 0) { + int session = read(data, offset); + frame.sessions.append(session); + + num_sessions--; + } + + return frame; +} + +QByteArray SAProtocol::packServiceConnectionResponseFrame(const ServiceConnectionResponseFrame &frame) +{ + QByteArray data; + + append(data, frame.messageType); + append(data, frame.acceptorId); + append(data, frame.initiatorId); + + data.append(frame.profile.toUtf8()); + data.append(';'); + + append(data, frame.statusCode); + + append(data, frame.sessions.size()); + + foreach (quint16 session, frame.sessions) { + append(data, session); + } + + return data; +} + +SAProtocol::CapabilityDiscoveryQuery SAProtocol::unpackCapabilityDiscoveryQuery(const QByteArray &data) +{ + CapabilityDiscoveryQuery msg; + int offset = 0; + + msg.messageType = static_cast(read(data, offset)); + msg.queryType = read(data, offset); + + msg.checksum = read(data, offset); + + int num_records = read(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(data, msg.messageType); + append(data, msg.queryType); + append(data, msg.checksum); + + append(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(read(data, offset)); + msg.queryType = read(data, offset); + + msg.checksum = read(data, offset); + + int num_records = read(data, offset); + + while (num_records > 0) { + CapabilityDiscoveryProvider provider; + + provider.uuid = read(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(data, offset); + + while (num_services > 0) { + CapabilityDiscoveryService service; + + service.componentId = read(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(data, offset); + service.role = read(data, offset); + service.connTimeout = read(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(data, msg.messageType); + append(data, msg.queryType); + append(data, msg.checksum); + + append(data, msg.providers.size()); + + foreach (const CapabilityDiscoveryProvider &provider, msg.providers) { + append(data, provider.uuid); + data.append(provider.name.toUtf8()); + data.append(';'); + append(data, provider.services.size()); + + foreach (const CapabilityDiscoveryService &service, provider.services) { + append(data, service.componentId); + data.append(service.profile.toUtf8()); + data.append(';'); + append(data, service.aspVersion); + append(data, service.role); + append(data, service.connTimeout); + } + } + + return data; +} -- cgit v1.2.3