From a45977185a485624095bff1a15024e9199eee676 Mon Sep 17 00:00:00 2001 From: Javier Date: Fri, 1 Jan 2016 22:05:42 +0100 Subject: reorganize source files into SAP and agents --- sap/saprotocol.cc | 651 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 651 insertions(+) create mode 100644 sap/saprotocol.cc (limited to 'sap/saprotocol.cc') 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 +#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(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, 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(data, offset); + } + if (withFragStatus) { + frame.fragStatus = static_cast(read(data, offset)); + } + frame.data = data.mid(offset); + return frame; +} + +QByteArray SAProtocol::packDataFrame(const DataFrame &frame) +{ + QByteArray data; + if (frame.withSeqNum) { + append(data, frame.seqNum); + } + if (frame.withFragStatus) { + append(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(read(data, offset)); + + switch (frame.type) { + case ControlFrameImmediateAck: + case ControlFrameBlockAck: + frame.seqNum = read(data, offset); + break; + case ControlFrameNak: + num_ranges = read(data, offset); + frame.seqNums.reserve(num_ranges); + for (int i = 0; i < num_ranges; i++) { + SeqNumRange r; + r.first = read(data, offset); + r.second = read(data, offset); + frame.seqNums.append(r); + } + break; + default: + qWarning() << "Unknown frame type"; + break; + } + + return frame; +} + +QByteArray SAProtocol::packControlFrame(const ControlFrame &frame) +{ + QByteArray data; + append(data, frame.type); + switch (frame.type) { + case ControlFrameImmediateAck: + case ControlFrameBlockAck: + Q_ASSERT(frame.seqNums.empty()); + append(data, frame.seqNum); + break; + case ControlFrameNak: + append(data, frame.seqNums.size()); + foreach (const SeqNumRange &r, frame.seqNums) { + append(data, r.first); + append(data, r.second); + } + break; + } + return data; +} + +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; + + const int num_sessions = read(data, offset); + frame.sessions.reserve(num_sessions); + + for (int i = 0; i < num_sessions; i++) { + ServiceConnectionRequestSession session; + session.sessionId = read(data, offset); + frame.sessions.append(session); + } + + for (int i = 0; i < num_sessions; i++) { + ServiceConnectionRequestSession &session = frame.sessions[i]; + session.channelId = read(data, offset); + } + + for (int i = 0; i < num_sessions; i++) { + ServiceConnectionRequestSession &session = frame.sessions[i]; + session.qosType = read(data, offset); + session.qosDataRate = read(data, offset); + session.qosPriority = read(data, offset); + } + + for (int i = 0; i < num_sessions; i++) { + ServiceConnectionRequestSession &session = frame.sessions[i]; + session.payloadType = read(data, offset); + } + + 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); + } + + foreach (const ServiceConnectionRequestSession &session, frame.sessions) { + append(data, session.channelId); + } + + foreach (const ServiceConnectionRequestSession &session, frame.sessions) { + append(data, session.qosType); + append(data, session.qosDataRate); + append(data, session.qosPriority); + } + + foreach (const ServiceConnectionRequestSession &session, frame.sessions) { + 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::ServiceTerminationRequestFrame SAProtocol::unpackServiceTerminationRequestFrame(const QByteArray &data) +{ + ServiceTerminationRequestFrame 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 termination request"; + return frame; + } + + frame.profile = QString::fromUtf8(&data.constData()[offset], marker - offset); + + return frame; +} + +QByteArray SAProtocol::packServiceTerminationRequestFrame(const ServiceTerminationRequestFrame &frame) +{ + QByteArray data; + + append(data, frame.messageType); + append(data, frame.acceptorId); + append(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(data, offset); + frame.acceptorId = read(data, offset); + frame.initiatorId = read(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(data, offset); + + return frame; +} + +QByteArray SAProtocol::packServiceTerminationResponseFrame(const ServiceTerminationResponseFrame &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); + + 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