src/test/unit/msp_serial_unittest.cc

   1 /*
   2  *
   3  * Cleanflight is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 3 of the License, or
   6  * (at your option) any later version.
   7  *
   8  * Cleanflight is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  15  */
  16
  17 #include <stdint.h>
  18 #include <string.h>
  19 #include <math.h>
  20
  21 //#define DEBUG_MSP
  22
  23 extern "C" {
  24     #include <platform.h>
  25     #include "build/build_config.h"
  26     #include "build/version.h"
  27     #include "build/debug.h"
  28
  29     #include "common/streambuf.h"
  30     #include "common/utils.h"
  31
  32     #include "config/parameter_group.h"
  33     #include "config/parameter_group_ids.h"
  34     #include "config/config_eeprom.h"
  35     #include "config/profile.h"
  36
  37     #include "drivers/system.h"
  38     #include "drivers/sensor.h"
  39     #include "drivers/accgyro.h"
  40     #include "drivers/compass.h"
  41     #include "drivers/serial.h"
  42     #include "drivers/serial_softserial.h"
  43     #include "drivers/buf_writer.h"
  44
  45     #include "rx/rx.h"
  46
  47     #include "io/serial.h"
  48     #include "msp/msp.h"
  49     #include "msp/msp_protocol.h"
  50     #include "msp/msp_serial.h"
  51
  52     #include "fc/runtime_config.h"
  53 }
  54
  55 #include "unittest_macros.h"
  56 #include "gtest/gtest.h"
  57
  58
  59 extern "C" {
  60     void mspSerialProcessReceivedCommand(mspPort_t *msp);
  61     extern mspPort_t mspPorts[];
  62
  63     PG_REGISTER(serialConfig_t, serialConfig, PG_SERIAL_CONFIG, 0);
  64 }
  65
  66 typedef struct mspHeader_s {
  67     uint8_t dollar;
  68     uint8_t m;
  69     uint8_t direction;
  70     uint8_t size;
  71     uint8_t type;
  72 } mspHeader_t;
  73
  74 #define SERIAL_BUFFER_SIZE 256
  75 typedef union mspBuffer_u {
  76     struct {
  77         mspHeader_t header;
  78         uint8_t payload[];
  79     };
  80     uint8_t buf[SERIAL_BUFFER_SIZE];
  81 } mspBuffer_t;
  82
  83 static mspBuffer_t serialWriteBuffer;
  84 static int serialWritePos = 0;
  85
  86 static mspBuffer_t serialReadBuffer;
  87 static int serialReadPos = 0;
  88 static int serialReadEnd = 0;
  89
  90 serialPort_t serialTestInstance;
  91
  92 void serialWrite(serialPort_t *instance, uint8_t ch)
  93 {
  94     EXPECT_EQ(instance, &serialTestInstance);
  95     EXPECT_LT(serialWritePos, sizeof(serialWriteBuffer.buf));
  96     serialWriteBuffer.buf[serialWritePos++] = ch;
  97 }
  98
  99 void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count)
 100 {
 101     while(count--)
 102         serialWrite(instance, *data++);
 103 }
 104
 105 void serialBeginWrite(serialPort_t *instance)
 106 {
 107     EXPECT_EQ(instance, &serialTestInstance);
 108 }
 109
 110 void serialEndWrite(serialPort_t *instance)
 111 {
 112     EXPECT_EQ(instance, &serialTestInstance);
 113 }
 114
 115 uint8_t serialRxBytesWaiting(serialPort_t *instance)
 116 {
 117     EXPECT_EQ(instance, &serialTestInstance);
 118     EXPECT_GE(serialReadEnd, serialReadPos);
 119     int ret = serialReadEnd - serialReadPos;
 120     if(ret >= 0) return ret;
 121     return 0;
 122 }
 123
 124 uint8_t serialRead(serialPort_t *instance)
 125 {
 126     EXPECT_EQ(instance, &serialTestInstance);
 127     EXPECT_LT(serialReadPos, serialReadEnd);
 128     const uint8_t ch = serialReadBuffer.buf[serialReadPos++];
 129     return ch;
 130 }
 131
 132 bool isSerialTransmitBufferEmpty(serialPort_t *instance)
 133 {
 134     EXPECT_EQ(instance, &serialTestInstance);
 135     return true;
 136 }
 137
 138 void serialTestResetBuffers()
 139 {
 140     memset(&serialReadBuffer.buf, 0, sizeof(serialReadBuffer.buf));
 141     serialReadPos = 0;
 142     serialReadEnd = 0;
 143     memset(&serialWriteBuffer.buf, 0, sizeof(serialWriteBuffer.buf));
 144     serialWritePos = 0;
 145 }
 146
 147 // dummy MSP command processor
 148 #define MSP_TEST_ECHO        1
 149 #define MSP_TEST_COMMAND     2
 150 #define MSP_TEST_REPLY       3
 151 #define MSP_TEST_ERROR       4
 152
 153 uint8_t msp_echo_data[]="PING\0PONG";
 154 uint8_t msp_request_data[]={0xbe, 0xef};
 155 uint8_t msp_reply_data[]={0x55,0xaa};
 156
 157 int mspServerCommandHandler(mspPacket_t *command, mspPacket_t *reply)
 158 {
 159     sbuf_t *src = &command->buf;
 160     sbuf_t *dst = &reply->buf;
 161     int cmdLength = sbufBytesRemaining(src);
 162     reply->cmd = command->cmd;
 163     switch(command->cmd) {
 164         case MSP_TEST_ECHO:
 165             while(sbufBytesRemaining(src) > 0)
 166                 sbufWriteU8(dst, sbufReadU8(src));
 167             break;
 168         case MSP_TEST_COMMAND:
 169             EXPECT_EQ(sizeof(msp_request_data), cmdLength);
 170             EXPECT_EQ(0, memcmp(sbufPtr(src), msp_request_data, sizeof(msp_request_data)));
 171             break;
 172         case MSP_TEST_REPLY:
 173             EXPECT_EQ(0, cmdLength);
 174             sbufWriteData(dst, msp_reply_data, sizeof(msp_reply_data));
 175             break;
 176         case MSP_TEST_ERROR:
 177             return -1;
 178     }
 179     return 1;
 180 }
 181
 182 int mspClientReplyHandler(mspPacket_t *command, mspPacket_t *reply)
 183 {
 184     UNUSED(command);
 185     UNUSED(reply);
 186
 187     // currently untested
 188     return -1;
 189 }
 190
 191 class SerialMspUnitTest : public ::testing::Test {
 192 protected:
 193     mspPort_t *mspPort;
 194     virtual void SetUp() {
 195         mspPort = &mspPorts[0];
 196         mspPort->port = &serialTestInstance;
 197         serialTestResetBuffers();
 198     }
 199 };
 200
 201 static uint8_t csumData(uint8_t csum, uint8_t* data, int len)
 202 {
 203     while(len--)
 204         csum ^= *data++;
 205     return csum;
 206 }
 207
 208 TEST_F(SerialMspUnitTest, Test_MspSerialOutFraming)
 209 {
 210     mspPort->cmdMSP = MSP_TEST_REPLY;
 211     mspPort->dataSize = 0;
 212     mspSerialProcessReceivedCommand(mspPort);
 213
 214     EXPECT_EQ('$', serialWriteBuffer.header.dollar);
 215     EXPECT_EQ('M', serialWriteBuffer.header.m);
 216     EXPECT_EQ('>', serialWriteBuffer.header.direction);
 217     EXPECT_EQ(sizeof(msp_reply_data), serialWriteBuffer.header.size);
 218     EXPECT_EQ(MSP_TEST_REPLY, serialWriteBuffer.header.type);
 219     for(unsigned i = 0; i < sizeof(msp_reply_data); i++)
 220         EXPECT_EQ(msp_reply_data[i], serialWriteBuffer.payload[i]);
 221     uint8_t checksum = sizeof(msp_reply_data) ^ MSP_TEST_REPLY;
 222     checksum = csumData(checksum, msp_reply_data, sizeof(msp_reply_data));
 223     EXPECT_EQ(checksum, serialWriteBuffer.payload[sizeof(msp_reply_data)]);  // checksum
 224 }
 225
 226 TEST_F(SerialMspUnitTest, Test_TestMspSerialInFraming)
 227 {
 228     uint8_t pkt[] = {'$', 'M', '<', sizeof(msp_request_data), MSP_TEST_COMMAND};
 229
 230     sbuf_t pbuf = {
 231         .ptr = serialReadBuffer.buf,
 232         .end = ARRAYEND(serialReadBuffer.buf),
 233     };
 234
 235     sbufWriteData(&pbuf, pkt, sizeof(pkt));
 236     sbufWriteData(&pbuf, msp_request_data, sizeof(msp_request_data));
 237
 238     uint8_t csum = 0;
 239     csum = pkt[3] ^ pkt[4];
 240     csum = csumData(csum, msp_request_data, sizeof(msp_request_data));
 241     sbufWriteU8(&pbuf, csum);
 242
 243     serialReadEnd = sbufPtr(&pbuf) - serialReadBuffer.buf;
 244
 245     mspSerialProcess();
 246
 247     EXPECT_EQ('$', serialWriteBuffer.header.dollar);
 248     EXPECT_EQ('M', serialWriteBuffer.header.m);
 249     EXPECT_EQ('>', serialWriteBuffer.header.direction);
 250     EXPECT_EQ(0, serialWriteBuffer.header.size);
 251     EXPECT_EQ(MSP_TEST_COMMAND, serialWriteBuffer.header.type);
 252     uint8_t checksum = 0 ^ MSP_TEST_COMMAND;
 253     EXPECT_EQ(checksum, serialWriteBuffer.payload[0]);
 254 }
 255
 256 // STUBS
 257 extern "C" {
 258 void evaluateOtherData(serialPort_t *, uint8_t) {}
 259 void handleOneshotFeatureChangeOnRestart(void) {}
 260 void stopMotors(void) {}
 261 uint8_t armingFlags = 0;
 262 void delay(uint32_t ms) {UNUSED(ms);}
 263 // from system_stm32fN0x.c
 264 void systemReset(void) {}
 265 void systemResetToBootloader(void) {}
 266 // from serial port drivers
 267 serialPort_t *usbVcpOpen(void) { return NULL; }
 268 serialPort_t *uartOpen(USART_TypeDef *, serialReceiveCallbackPtr, uint32_t, portMode_t, portOptions_t) { return NULL; }
 269 serialPort_t *openSoftSerial(softSerialPortIndex_e, serialReceiveCallbackPtr, uint32_t, portOptions_t) { return NULL; }
 270 void serialSetMode(serialPort_t *, portMode_t) {}
 271 bool isRebootScheduled = false;
 272 }
 273