113 lines
3.3 KiB
C
113 lines
3.3 KiB
C
#include "message_parser.h"
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
MessageReceivedCallback on_message_received = NULL;
|
|
MessageFailCallback on_message_fail = NULL;
|
|
|
|
struct MessageReceive InitMessageReceive() {
|
|
struct MessageReceive mr = {
|
|
.state = WaitingForStartByte, // Startzustand des Parsers
|
|
.error = NoError, // Kein Fehler zu Beginn
|
|
.messageid = 0, // MSGID auf Standardwert setzen
|
|
// .message Array muss nicht explizit initialisiert werden, da es bei
|
|
// jedem Start geleert wird
|
|
.index = 0, // Index für das Nachrichten-Array initialisieren
|
|
.checksum = 0 // Checksumme initialisieren
|
|
};
|
|
return mr;
|
|
}
|
|
|
|
// Registrierungsfunktionen für die Callbacks
|
|
void register_message_callback(MessageReceivedCallback callback) {
|
|
on_message_received = callback;
|
|
}
|
|
|
|
void register_message_fail_callback(MessageFailCallback callback) {
|
|
on_message_fail = callback;
|
|
}
|
|
|
|
void parse_byte(struct MessageReceive *mr, uint8_t pbyte) {
|
|
switch (mr->state) {
|
|
case WaitingForStartByte:
|
|
if (pbyte == StartByte) {
|
|
mr->index = 0;
|
|
mr->checksum = 0;
|
|
mr->state = GetMessageType;
|
|
}
|
|
break;
|
|
case EscapedMessageType:
|
|
mr->messageid = pbyte;
|
|
mr->checksum ^= pbyte;
|
|
mr->state = InPayload;
|
|
break;
|
|
case GetMessageType:
|
|
if (pbyte == EscapeByte) {
|
|
mr->state = EscapedMessageType;
|
|
return;
|
|
}
|
|
if (pbyte == StartByte || pbyte == EndByte) {
|
|
mr->state = WaitingForStartByte;
|
|
mr->error = UnexpectedCommandByte;
|
|
if (on_message_received) {
|
|
on_message_fail(mr->messageid, mr->message, mr->index, mr->error);
|
|
}
|
|
return;
|
|
}
|
|
mr->messageid = pbyte;
|
|
mr->checksum ^= pbyte;
|
|
mr->state = InPayload;
|
|
break;
|
|
case EscapePayloadByte:
|
|
mr->message[mr->index++] = pbyte;
|
|
mr->checksum ^= pbyte;
|
|
mr->state = InPayload;
|
|
break;
|
|
case InPayload:
|
|
if (pbyte == EscapeByte) {
|
|
mr->state = EscapePayloadByte;
|
|
return;
|
|
}
|
|
if (pbyte == StartByte) {
|
|
mr->state = WaitingForStartByte;
|
|
mr->error = UnexpectedCommandByte;
|
|
if (on_message_received) {
|
|
on_message_fail(mr->messageid, mr->message, mr->index, mr->error);
|
|
}
|
|
return;
|
|
}
|
|
if (pbyte == EndByte) {
|
|
if (mr->checksum != 0x00) {
|
|
// Checksum failure
|
|
// The Checksum gets treated like a normal byte until the end byte
|
|
// accours. Therefore the last byte xor'ed to the checksum ist the
|
|
// checksum so the checksum must be Zero.
|
|
mr->state = WaitingForStartByte;
|
|
mr->error = WrongCheckSum;
|
|
if (on_message_received) {
|
|
on_message_fail(mr->messageid, mr->message, mr->index, mr->error);
|
|
}
|
|
return;
|
|
}
|
|
if (on_message_received) {
|
|
on_message_received(mr->messageid, mr->message,
|
|
mr->index - 1); // remove checksum byte by just
|
|
// setting the length of the message
|
|
}
|
|
mr->state = WaitingForStartByte;
|
|
}
|
|
if (mr->index < MAX_TOTAL_CONTENT_LENGTH) {
|
|
mr->message[mr->index++] = pbyte;
|
|
mr->checksum ^= pbyte;
|
|
} else {
|
|
mr->state = WaitingForStartByte;
|
|
mr->error = MessageToLong;
|
|
if (on_message_received) {
|
|
on_message_fail(mr->messageid, mr->message, mr->index, mr->error);
|
|
}
|
|
return;
|
|
}
|
|
break;
|
|
}
|
|
}
|