xm5-linux/message.cpp at main · muhammad23012009/xm5-linux · GitHub

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
#include <message.h>

Message::Message(MessageType type, int sequenceNumber, const QByteArray &payload):
  m_type(type),
  m_sequence(sequenceNumber),
  m_data(payload)
{
}

Message::Message(MessageType type, const QByteArray &payload):
  m_type(type),
  m_data(payload)
{
}

Message::Message(const QByteArray &message)
{
    if (message.at(0) != MESSAGE_HEADER) {
        qWarning() << "Invalid message!";
        throw;
    }

    if (message.at(message.length() - 1) != MESSAGE_TRAILER) {
        qWarning() << "INVALID MESSAGE!";
        throw;
    }

    QByteArray data = unescape(message);
    quint8 messageChecksum = data.at(data.length() - 2);
    quint8 expectedChecksum = calculateChecksum(data, 1, data.length() - 2);

    if (messageChecksum != expectedChecksum) {
        qWarning() << "Invalid checksum!";
        throw;
    }

    quint32 length = payloadLength(data);

    m_type = (MessageType)data.at(1);
    m_sequence = data.at(2);
    m_data = QByteArray(&data.constData()[7], length);
}

Message::~Message()
{
}

Message::MessageType Message::type() const
{
    return m_type;
}

int Message::sequence() const
{
    return m_sequence;
}

QByteArray Message::data() const
{
    return m_data;
}

QByteArray Message::encode()
{
    return encode(m_sequence);
}

QByteArray Message::encode(int sequenceNumber)
{
    QByteArray out;

    out.append(m_type);
    out.append(sequenceNumber & 0xFF);

    out.append((m_data.length() & 0xFF000000) >> 24);
    out.append((m_data.length() & 0xFF0000) >> 16);
    out.append((m_data.length() & 0xFF00) >> 8);
    out.append(m_data.length() & 0xFF);

    if (!m_data.isEmpty() && !m_data.isNull())
        out.append(m_data);

    return encodeMessage(out);
}

// private API
QByteArray Message::escape(const QByteArray &message)
{
    QByteArray output;
    for (quint8 byte : message) {
        switch (byte) {
            case MESSAGE_HEADER:
            case MESSAGE_TRAILER:
            case MESSAGE_ESCAPE:
                output.append(MESSAGE_ESCAPE);
                output.append(byte & MESSAGE_ESCAPE_MASK);
                break;
            default:
                output.append(byte);
        }
    }
    return output;
}

QByteArray Message::escape(const quint8 byte)
{
    return escape(QByteArray(1, byte));
}

QByteArray Message::unescape(const QByteArray &message)
{
    QByteArray out;
    for (int i = 0; i < message.length(); i++) {
        if (message[i] == MESSAGE_ESCAPE) {
            if (++i >= message.length()) {
                qWarning() << "Too short message!";
                return QByteArray();
            }
            out.append(message[i] | ~MESSAGE_ESCAPE_MASK);
        } else {
            out.append(message[i]);
        }
    }

    return out;
}

quint8 Message::calculateChecksum(const QByteArray &message, int start, int length)
{
    int chk = 0;
    for (int i = start; i < length; i++) {
        chk += message.at(i) & 255;
    }
    return chk;
}

QByteArray Message::encodeMessage(const QByteArray &message)
{
    QByteArray out;
    quint8 checksum = calculateChecksum(message, 0, message.length());

    out.append(MESSAGE_HEADER);

    out.append(escape(message));
    out.append(escape(checksum));

    out.append(MESSAGE_TRAILER);

    return out;
}

quint32 Message::payloadLength(const QByteArray &data)
{
    quint32 packetLength = qFromBigEndian<quint32>(&data.constData()[3]);
    if (packetLength != data.length() - 9) {
        qWarning() << "Invalid packet length - proceed with caution!";
    }

    return packetLength;
}