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; }