From 2b63d4a0606cf8d08540c1f2ffb3da4d7f7ed1db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Fri, 3 Jan 2020 15:32:39 +0000 Subject: [PATCH] [MSP] Add support for MSP2_COMMON_SERIAL_CONFIG / MSP2_COMMON_SET_SERIAL_CONFIG This allows retrieving ports with functionMask with values >= (1 << 16) while still keeping MSP_CF_SERIAL_CONFIG / MSP_SET_CF_SERIAL_CONFIG backwards compatible. The new handler includes the port count at the start of the frame, so the per-port data size can be extended in a backwards compatible way in the future. --- src/main/msp/msp.c | 66 ++++++++++++++++++++++++--- src/main/msp/msp_protocol_v2_common.h | 22 +++++++++ 2 files changed, 82 insertions(+), 6 deletions(-) create mode 100644 src/main/msp/msp_protocol_v2_common.h diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index bf3e9e8f0f..495a6504ad 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -103,6 +103,7 @@ #include "msp/msp_box.h" #include "msp/msp_protocol.h" +#include "msp/msp_protocol_v2_common.h" #include "msp/msp_serial.h" #include "osd/osd.h" @@ -558,7 +559,7 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin * Returns true if the command was processd, false otherwise. * May set mspPostProcessFunc to a function to be called once the command has been processed */ -static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) +static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) { UNUSED(mspPostProcessFn); @@ -946,7 +947,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce return true; } -static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) +static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) { bool unsupportedCommand = false; @@ -1465,6 +1466,27 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex); } break; + case MSP2_COMMON_SERIAL_CONFIG: { + uint8_t count = 0; + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + if (serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) { + count++; + } + } + sbufWriteU8(dst, count); + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) { + continue; + }; + sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier); + sbufWriteU32(dst, serialConfig()->portConfigs[i].functionMask); + sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex); + } + break; + } #ifdef USE_LED_STRIP_STATUS_MODE case MSP_LED_COLORS: @@ -1840,7 +1862,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) return !unsupportedCommand; } -static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) +static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) { switch (cmdMSP) { @@ -2026,7 +2048,7 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) } #endif -static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src) +static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src) { uint32_t i; uint8_t value; @@ -3035,6 +3057,38 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, } } break; + case MSP2_COMMON_SET_SERIAL_CONFIG: { + if (dataSize < 1) { + return MSP_RESULT_ERROR; + } + unsigned count = sbufReadU8(src); + unsigned portConfigSize = (dataSize - 1) / count; + unsigned expectedPortSize = sizeof(uint8_t) + sizeof(uint32_t) + (sizeof(uint8_t) * 4); + if (portConfigSize < expectedPortSize) { + return MSP_RESULT_ERROR; + } + for (unsigned ii = 0; ii < count; ii++) { + unsigned start = sbufBytesRemaining(src); + uint8_t identifier = sbufReadU8(src); + serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier); + + if (!portConfig) { + return MSP_RESULT_ERROR; + } + + portConfig->identifier = identifier; + portConfig->functionMask = sbufReadU32(src); + portConfig->msp_baudrateIndex = sbufReadU8(src); + portConfig->gps_baudrateIndex = sbufReadU8(src); + portConfig->telemetry_baudrateIndex = sbufReadU8(src); + portConfig->blackbox_baudrateIndex = sbufReadU8(src); + // Skip unknown bytes + while (start - sbufBytesRemaining(src) < portConfigSize && sbufBytesRemaining(src)) { + sbufReadU8(src); + } + } + break; + } #ifdef USE_LED_STRIP_STATUS_MODE case MSP_SET_LED_COLORS: @@ -3157,7 +3211,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, return MSP_RESULT_ACK; } -static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) +static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) { UNUSED(mspPostProcessFn); const unsigned int dataSize = sbufBytesRemaining(src); @@ -3412,7 +3466,7 @@ mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPa int ret = MSP_RESULT_ACK; sbuf_t *dst = &reply->buf; sbuf_t *src = &cmd->buf; - const uint8_t cmdMSP = cmd->cmd; + const int16_t cmdMSP = cmd->cmd; // initialize reply by default reply->cmd = cmd->cmd; diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h new file mode 100644 index 0000000000..2031df2bbe --- /dev/null +++ b/src/main/msp/msp_protocol_v2_common.h @@ -0,0 +1,22 @@ +/* + * This file is part of Cleanflight, Betaflight and INAV. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight, Betaflight and INAV are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#define MSP2_COMMON_SERIAL_CONFIG 0x1009 +#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A