86 lines
2.4 KiB
C
86 lines
2.4 KiB
C
#include "proto/msg.h"
|
|
#include "panic.h"
|
|
|
|
#include "proto/enums.h"
|
|
#include "proto/pack.h"
|
|
|
|
#include <string.h>
|
|
|
|
BaseMessage* parseMessage(const uint8_t* bytes, size_t size)
|
|
{
|
|
const uint8_t headerSize = 9;
|
|
BaseMessage* msg = malloc(sizeof(BaseMessage));
|
|
NULL_GUARD(msg);
|
|
uint64_t nonce = decodeBytesToU64(bytes);
|
|
|
|
|
|
msg->nonce = nonce;
|
|
|
|
msg->packetType = bytes[8] >> 4;
|
|
msg->payloadHeader = bytes[8] & 0b1111;
|
|
|
|
msg->payloadLen = size - headerSize;
|
|
|
|
uint8_t* payload = malloc(sizeof(uint8_t) * (msg->payloadLen));
|
|
NULL_GUARD(payload);
|
|
|
|
memcpy(payload, bytes + headerSize, msg->payloadLen);
|
|
msg->payload = payload;
|
|
|
|
return msg;
|
|
}
|
|
|
|
|
|
uint8_t* createControlNotifyMessage(uint64_t nonce, uint64_t clockCounter, uint8_t newEmulState, size_t* lenOut)
|
|
{
|
|
*lenOut = 9 + 10;
|
|
uint8_t* outmsg = malloc(sizeof(uint8_t) * (*lenOut));
|
|
NULL_GUARD(outmsg, "Unable to allocate message");
|
|
|
|
encodeUintToBytes(nonce, outmsg);
|
|
|
|
outmsg[8] = PACKET_TYPE_CTRL << 4;
|
|
outmsg[8] |= CTRL_TYPE_NOTIF_STATE;
|
|
|
|
outmsg[9] = NOTIF_TYPE_EXEC;
|
|
encodeUintToBytes(clockCounter, outmsg + 10);
|
|
outmsg[18] = newEmulState;
|
|
return outmsg;
|
|
}
|
|
|
|
|
|
uint8_t* createDoneRegMessage(uint64_t nonce, uint8_t X, uint64_t devId, uint64_t segId, uint64_t startAddr, uint64_t segLength, uint32_t regId, size_t* lenOut)
|
|
{
|
|
*lenOut = 36 + 9;
|
|
uint8_t* outmsg = malloc(sizeof(uint8_t) * (*lenOut));
|
|
NULL_GUARD(outmsg);
|
|
|
|
encodeUintToBytes(nonce, outmsg);
|
|
outmsg[8] = (uint8_t)((PACKET_TYPE_STREAM << 4) | (X << 3) | STREAM_TYPE_REG_CONFIRM);
|
|
encodeUintToBytes(devId, outmsg + 9);
|
|
encodeUintToBytes(segId, outmsg + 9 + 8);
|
|
encodeUintToBytes(startAddr, outmsg + 9 + 8 + 8);
|
|
encodeUintToBytes(segLength, outmsg + 9 + 8 + 8 + 8);
|
|
encodeUintToBytes(regId, outmsg + 9 + 8 + 8 + 8 + 8);
|
|
return outmsg;
|
|
}
|
|
|
|
|
|
uint8_t* createStreamSegmentPush(uint8_t mode, uint32_t regId, uint64_t clockCounter, uint8_t* payload, size_t payloadLen, size_t* lenOut)
|
|
{
|
|
*lenOut = 9 + 4 + 8 + payloadLen;
|
|
|
|
uint8_t* outmsg = malloc(sizeof(uint8_t) * (*lenOut));
|
|
NULL_GUARD(outmsg);
|
|
|
|
uint64_t nonce = (uint64_t)~0;
|
|
encodeUintToBytes(nonce, outmsg);
|
|
outmsg[8] = (uint8_t)((PACKET_TYPE_STREAM << 4) | (mode << 3) | STREAM_TYPE_SEND);
|
|
|
|
encodeUintToBytes(regId, outmsg + 9);
|
|
encodeUintToBytes(clockCounter, outmsg + 9 + 4);
|
|
memcpy(outmsg + 9 + 4 + 8, payload, payloadLen);
|
|
|
|
return outmsg;
|
|
}
|