static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn)
{
static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
mspPacket_t reply = {
.buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
.cmd = -1,
.flags = 0,
.result = 0,
.direction = MSP_DIRECTION_REPLY,
};
uint8_t *outBufHead = reply.buf.ptr;
mspPacket_t command = {
.buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
.cmd = msp->cmdMSP,
.flags = msp->cmdFlags,
.result = 0,
.direction = MSP_DIRECTION_REQUEST,
};
mspPostProcessFnPtr mspPostProcessFn = NULL;
const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
if (status != MSP_RESULT_NO_REPLY) {
sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
mspSerialEncode(msp, &reply, msp->mspVersion);
}
return mspPostProcessFn;
}
STATIC_UNIT_TESTED void mspSerialProcessReceivedCommand(mspPort_t *msp)
{
uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
mspPacket_t message = {
.buf = {
.ptr = outBuf,
.end = ARRAYEND(outBuf),
},
.cmd = -1,
.result = 0,
};
mspPacket_t command = {
.buf = {
.ptr = msp->inBuf,
.end = msp->inBuf + msp->dataSize,
},
.cmd = msp->cmdMSP,
.result = 0,
};
mspPacket_t *reply = &message;
uint8_t *outBufHead = reply->buf.ptr;
int status = mspProcessCommand(&command, reply);
if (status) {
// reply should be sent back
sbufSwitchToReader(&reply->buf, outBufHead); // change streambuf direction
mspSerialEncode(msp, reply);
}
msp->c_state = IDLE;
}