From 4aa377673f684adfbc447cda4a44e0a3b989b1b4 Mon Sep 17 00:00:00 2001 From: Raphael Hunziker Date: Thu, 10 Sep 2026 21:43:03 +0200 Subject: [PATCH] msp-over-telemetry: restart reply framing when a reply is discarded sendMspReply() kept the "header already sent" flag in a function local static that is cleared only after the last chunk of a reply. When a new request arrives while a reply is still being sent, handleMspFrame() discards that reply through initSharedMsp(), but the flag stayed set, so the first chunk of the next reply went out as a continuation frame: a status byte only, without the payload size and without the function byte. A consumer cannot tell which request such a reply belongs to and parses the payload at the wrong offset. Both transports reach this: the SmartPort handler feeds every incoming MSP payload to handleMspFrame() while a reply is pending, and the CRSF buffer loop processes several queued requests in a row. Move the flag to the module state and clear it in initSharedMsp(), so a reply that starts after a discarded one carries a full header again: status, size, function for MSPv1, status, flags, function, size for MSPv2, matching the request layout described by MSP_INDEX_*. Send a reply that exactly fills the last frame as the final chunk as well, instead of announcing a further chunk that then carried nothing but a continuation status byte. Fixes #10667 --- src/main/telemetry/msp_shared.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index f6d4c6eafbd..c5c29ea4684 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -57,6 +57,7 @@ enum { // byte position(index) in msp-over-telemetry request payload }; static uint8_t lastRequestVersion; // MSP version of last request. Temporary solution. It's better to keep it in requestPacket. +static bool replyHeaderSent; // true once the header of the reply being sent has been emitted STATIC_UNIT_TESTED mspPackage_t mspPackage; static mspRxBuffer_t mspRxBuffer; static mspTxBuffer_t mspTxBuffer; @@ -74,6 +75,8 @@ void initSharedMsp(void) mspPackage.responsePacket = &mspTxPacket; mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer; mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + + replyHeaderSent = false; // the reply that was in progress (if any) is discarded, the next one starts with a header } static bool processMspPacket(void) @@ -206,7 +209,6 @@ bool handleMspFrame(uint8_t *const frameStart, const int payloadLength) bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) { static uint8_t seq = 0; - static bool headerSent = false; uint8_t payloadOut[payloadSize]; sbuf_t payload; @@ -214,7 +216,7 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) sbuf_t *txBuf = &mspPackage.responsePacket->buf; // detect first reply packet - if (!headerSent) { + if (!replyHeaderSent) { // header uint8_t status = TELEMETRY_MSP_START_MASK | (seq++ & TELEMETRY_MSP_SEQ_MASK) | (lastRequestVersion << TELEMETRY_MSP_VER_SHIFT);; @@ -232,7 +234,7 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) sbufWriteU16(payloadBuf, mspPackage.responsePacket->cmd); // command is 16 bit in MSPv2 sbufWriteU16(payloadBuf, (uint16_t) size); // size is 16 bit in MSPv2 } - headerSent = true; + replyHeaderSent = true; } else { sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK) | (lastRequestVersion << TELEMETRY_MSP_VER_SHIFT)); // header without 'start' flag } @@ -241,7 +243,7 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); uint8_t frame[payloadBytesRemaining]; - if (bufferBytesRemaining >= payloadBytesRemaining) { + if (bufferBytesRemaining > payloadBytesRemaining) { // frame is filled up and more data is left for the next one sbufReadData(txBuf, frame, payloadBytesRemaining); sbufAdvance(txBuf, payloadBytesRemaining); @@ -258,7 +260,7 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) sbufSwitchToReader(txBuf, mspPackage.responseBuffer); responseFn(payloadOut, payloadBuf->ptr - payloadOut); - headerSent = false; // <-- added: reset for the next response + replyHeaderSent = false; // reset for the next response return false; }