sdp-sniffer/src/MessageDecoder.cpp

//
// Created by vlad on 21.01.2022.
//

#include "MessageDecoder.h"

MessageDecoder::MessageDecoder(const MessageStruct &message) : msg(message) {}

int MessageDecoder::getGroup() const {
    if ((msg.stdId & 0x7C0) == 0x7C0) {
        // group = 4;
        return 4;
    } else if ((msg.stdId & 0x600) == 0x600) {
        // group = 3;
        return 3;
    } else if ((msg.stdId & 0x600) == 0x400) {
        // group = 2;
        return 2;
    } else if ((msg.stdId & 0x400) == 0) {
        // group = 1;
        return 1;
    } else {
        return -1;
    }
}

int MessageDecoder::getMsgId() const {
    switch (getGroup()) {
        case 1: return (msg.stdId & 0x3C0) >> 6;
        case 2: return msg.stdId & 0x7;
        case 3: return (msg.stdId & 0x1C0) >> 6;
        case 4: return msg.stdId & 0x3F;
    }

    return 0;
}

int MessageDecoder::getSrcMac() const {
    switch (getGroup()) {
        case 3:
        case 1: return msg.stdId & 0x3F;
        case 2:
            switch (getMsgId()) {
                case 0:
                case 1:
                case 3: return (msg.stdId & 0x1F8) >> 3;
                case 2:
                case 4:
                case 5:
                case 6: // 6-ой и 7-ой ID кстати не описан в том документе...
                case 7:
                    // данные должны быть, но не обязаны
                    if (msg.dataLen != 0)
                        return msg.data[0] & 0x3F;
                    else
                        return -1;
                default: return -1;
            }
        case 4:
            if (msg.dataLen != 0)
                return msg.data[0] & 0x3F;
            else
                return -1;
    }
    return -1;
}

int MessageDecoder::getDestMac() const {
    switch (getGroup()) {
        case 3:
        case 1:
            // там куча MsgId, но я ебал их в рот
            if (msg.dataLen != 0)
                return msg.data[0] & 0x3F;
            else
                return -1;
        case 2:
            switch (getMsgId()) {
                case 0:
                case 1:
                case 3:
                    if (msg.dataLen != 0)
                        return msg.data[0] & 0x3F;
                    else
                        return -1;
                case 2:
                case 4:
                case 5:
                case 6: // 6-ой и 7-ой ID кстати не описан в том документе...
                case 7:
                    // данные должны быть, но не обязаны
                    return (msg.stdId & 0x1F8) >> 3;
                default: return -1;
            }
        case 4:
            return -1;
    }
    return -1;
}

std::string MessageDecoder::getMessageDescription() const {
    switch (getGroup()) {
        case 1:
            switch (getMsgId()) {
                case 0x8: return "Slave I/O Multicast poll response";
                case 0xC: return "Slave I/O Change of state or cyclic message";
                case 0xE: return "Slave I/O Bit-Strobe response message";
                case 0xF: return "Slave I/O Poll response or COS/Cyclic Ack message";
                default: return "Group 1 message";
            }
        case 2:
            switch (getMsgId()) {
                case 0x0: return "Master I/O Bit-Strobe command message";
                case 0x1: return "Master I/O Multicast poll Group ID";
                case 0x2: return "Master Change of state or Cyclic Ack message";
                case 0x3: return "Slave Explicit/Unconnected response message";
                case 0x4: return "Master Explicit request message";
                case 0x5: return "Master I/O Poll command/COS/Cyclic message";
                    // а это уже из таблички из вк, потому что в документе эти сообщения не описаны
                case 0x6: return "Group 2 only Unconnected explicit request messages";
                case 0x7: return "Duplicate MAC-ID check message";
                default: return "Group 2 undefined message";
            }
        case 3: return "Group 3 message";
        case 4: return "Group 4 message";
    }
    return "undefined description";
}

static std::string buildDataArray(const MessageStruct& msg) {
    if (msg.dataLen == 0) return "{}";
    std::string out = "{";
    for (int i = 0; i < msg.dataLen; i++) {
        char tmp[8];
        if (i == msg.dataLen - 1) {
            sprintf(tmp, "0x%02X", msg.data[i]);
        } else {
            sprintf(tmp, "0x%02X ", msg.data[i]);
        }
        out += tmp;
    }
    return out + "}";
}
static std::string buildExplicitMessageFrame(const MessageStruct& msg) {
    if (msg.dataLen == 0) return "empty explicit frame";
    char buff[256];
    int headerLen;
    if (msg.data[0] & 0x80) {
        // Frag=1
        sprintf(buff, "{{Frag=1, XID=%d, MAC=%d}, {FragType=%d, Fragment count=%d}, {R/R=%d, ServiceCode=%d} Data=",
                (msg.data[0] & 0x40) != 0, msg.data[0] & 0x3F,
                (msg.data[1] & 0xC0) >> 6, msg.data[1] & 0x3F,
                (msg.data[2] & 0x80) != 0, msg.data[2] & 0x7F);
        headerLen = 3;
    } else {
        sprintf(buff, "{{Frag=0, XID=%d, MAC=%d}, {R/R=%d, ServiceCode=%d} Data=",
                (msg.data[0] & 0x40) != 0, msg.data[0] & 0x3F,
                (msg.data[1] & 0x80) != 0, msg.data[1] & 0x7F);
        headerLen = 2;
    }

    MessageStruct tmpMsg(msg);
    if (tmpMsg.dataLen >= headerLen) {
        // убираем первый байт, чтобы посылка была
        tmpMsg.dataLen -= headerLen;
        for (int i = 0; i < tmpMsg.dataLen; i++) {
            tmpMsg.data[i] = tmpMsg.data[i + headerLen];
        }
    } else {
        return "invalid explicit frame " + buildDataArray(tmpMsg);
    }
    return buff + buildDataArray(tmpMsg) + "}";
}
static std::string buildBitIOStrobeFrame(const MessageStruct& msg) { return "bit IO strobe frame " + buildDataArray(msg); }
static std::string buildChangeOfStateFrame(const MessageStruct& msg) { return "change of state frame " + buildDataArray(msg); }
static std::string buildIOPollFrame(const MessageStruct& msg) { return "change of state frame " + buildDataArray(msg); }

std::string MessageDecoder::getDataFrameDescription() const {
    if (msg.dataLen == 0) {
        return "empty frame";
    } else {
        switch (getGroup()) {
            case 1:
                return "Group 1 frame (IO) " + buildDataArray(msg);
            case 2:
                switch (getMsgId()) {
                    case 0: return buildBitIOStrobeFrame(msg);
                    case 1: return buildIOPollFrame(msg);
                    case 2: return buildChangeOfStateFrame(msg);
                    case 3:
                    case 4: return buildExplicitMessageFrame(msg);
                    case 5: return buildIOPollFrame(msg);
                    case 6:
                    case 7: return buildExplicitMessageFrame(msg);
                }
                return "Group 2 frame" + buildDataArray(msg);
            case 3:
                return buildExplicitMessageFrame(msg);
            case 4:
                return "Group 4 frame " + buildDataArray(msg);
        }
    }
    return "unknown frame " + buildDataArray(msg);
}

std::string MessageDecoder::toString() const {
    char buff[1024];
    sprintf(buff, "// StdId=0x%03X, DataLen=%d, Group=%d, MsgId=%d, SrcMac=%d, DestMac=%d (%s)\n"
                  "// Frame: %s", msg.stdId, msg.dataLen, getGroup(), getMsgId(), getSrcMac(), getDestMac(),
            getMessageDescription().c_str(), getDataFrameDescription().c_str());
    return buff;
}