Line data    Source code

       1             : #include <AP_Math/AP_Math.h>
       2             : #include <AP_SerialManager/AP_SerialManager.h>
       3             : 
       4             : #include "msp.h"
       5             : 
       6             : #include <stdint.h>
       7             : 
       8             : #if HAL_MSP_ENABLED
       9             : 
      10             : /*
      11             :  ported from betaflight/src/main/msp/msp_serial.c
      12             :  */
      13       43944 : uint8_t MSP::msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, uint32_t len)
      14             : {
      15      256446 :     while (len-- > 0) {
      16      212502 :         checksum ^= *data++;
      17             :     }
      18       43944 :     return checksum;
      19             : }
      20             : 
      21             : /*
      22             :  ported from betaflight/src/main/msp/msp_serial.c
      23             :  */
      24       21982 : uint32_t MSP::msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32_t hdr_len, const uint8_t * data, uint32_t data_len, const uint8_t * crc, uint32_t crc_len)
      25             : {
      26       21982 :     if (msp->uart == nullptr) {
      27           0 :         return 0;
      28             :     }
      29             : 
      30             :     // only send the response if it fits in the transmit buffer. Writes do not
      31             :     // block, so writing a frame which does not fit would put a truncated frame
      32             :     // on the wire for the other end to choke on
      33       21982 :     const uint32_t total_frame_length = hdr_len + data_len + crc_len;
      34             : 
      35       21982 :     if (msp->uart->txspace() < total_frame_length) {
      36       19443 :         return 0;
      37             :     }
      38             : 
      39             :     // Transmit frame
      40        2539 :     msp->uart->write(hdr, hdr_len);
      41        2539 :     msp->uart->write(data, data_len);
      42        2539 :     msp->uart->write(crc, crc_len);
      43             : 
      44        2539 :     return total_frame_length;
      45             : }
      46             : 
      47             : /*
      48             :  ported from betaflight/src/main/msp/msp_serial.c
      49             :  */
      50       21972 : uint32_t MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version, bool is_request)
      51             : {
      52             :     static const uint8_t msp_magic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER;
      53             :     /*
      54             :         Note: after calling sbuf_switch_to_reader() sbuf_bytes_remaining() returns the size of the packet
      55             :     */
      56       21972 :     const uint16_t data_len = sbuf_bytes_remaining(&packet->buf);
      57             :     uint8_t code;
      58       21972 :     if (is_request) {
      59           0 :         code = '<';
      60       21972 :     } else if (packet->result == MSP_RESULT_ERROR) {
      61           0 :         code = '!';
      62             :     } else {
      63       21972 :         code = '>';
      64             :     }
      65       21972 :     const uint8_t hdr_buf[16] = { '$', msp_magic[msp_version], code };
      66             :     uint8_t crc_buf[2];
      67       21972 :     uint32_t hdr_len = 3;
      68       21972 :     uint32_t crc_len = 0;
      69             : 
      70             : #define V1_CHECKSUM_STARTPOS 3
      71       21972 :     if (msp_version == MSP_V1) {
      72       21972 :         msp_header_v1_t * hdr_v1 = (msp_header_v1_t *)&hdr_buf[hdr_len];
      73       21972 :         hdr_len += sizeof(msp_header_v1_t);
      74       21972 :         hdr_v1->cmd = packet->cmd;
      75             : 
      76             :         // Add JUMBO-frame header if necessary
      77       21972 :         if (data_len >= JUMBO_FRAME_SIZE_LIMIT) {
      78           0 :             msp_header_jumbo_t * hdr_jumbo = (msp_header_jumbo_t *)&hdr_buf[hdr_len];
      79           0 :             hdr_len += sizeof(msp_header_jumbo_t);
      80             : 
      81           0 :             hdr_v1->size = JUMBO_FRAME_SIZE_LIMIT;
      82           0 :             hdr_jumbo->size = data_len;
      83             :         } else {
      84       21972 :             hdr_v1->size = data_len;
      85             :         }
      86             : 
      87             :         // Pre-calculate CRC
      88       21972 :         crc_buf[crc_len] = msp_serial_checksum_buf(0, hdr_buf + V1_CHECKSUM_STARTPOS, hdr_len - V1_CHECKSUM_STARTPOS);
      89       21972 :         crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len);
      90       21972 :         crc_len++;
      91           0 :     } else if (msp_version == MSP_V2_OVER_V1) {
      92           0 :         msp_header_v1_t * hdr_v1 = (msp_header_v1_t *)&hdr_buf[hdr_len];
      93             : 
      94           0 :         hdr_len += sizeof(msp_header_v1_t);
      95             : 
      96           0 :         msp_header_v2_t * hdr_v2 = (msp_header_v2_t *)&hdr_buf[hdr_len];
      97           0 :         hdr_len += sizeof(msp_header_v2_t);
      98             : 
      99           0 :         const uint32_t v1_payload_size = sizeof(msp_header_v2_t) + data_len + 1;  // MSPv2 header + data payload + MSPv2 checksum
     100           0 :         hdr_v1->cmd = MSP_V2_FRAME_ID;
     101             : 
     102             :         // Add JUMBO-frame header if necessary
     103           0 :         if (v1_payload_size >= JUMBO_FRAME_SIZE_LIMIT) {
     104           0 :             msp_header_jumbo_t * hdr_jumbo = (msp_header_jumbo_t *)&hdr_buf[hdr_len];
     105           0 :             hdr_len += sizeof(msp_header_jumbo_t);
     106             : 
     107           0 :             hdr_v1->size = JUMBO_FRAME_SIZE_LIMIT;
     108           0 :             hdr_jumbo->size = v1_payload_size;
     109             :         } else {
     110           0 :             hdr_v1->size = v1_payload_size;
     111             :         }
     112             : 
     113             :         // Fill V2 header
     114           0 :         hdr_v2->flags = packet->flags;
     115           0 :         hdr_v2->cmd = packet->cmd;
     116           0 :         hdr_v2->size = data_len;
     117             : 
     118             :         // V2 CRC: only V2 header + data payload
     119           0 :         crc_buf[crc_len] = crc8_dvb_s2_update(0, (uint8_t *)hdr_v2, sizeof(msp_header_v2_t));
     120           0 :         crc_buf[crc_len] = crc8_dvb_s2_update(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len);
     121           0 :         crc_len++;
     122             : 
     123             :         // V1 CRC: All headers + data payload + V2 CRC byte
     124           0 :         crc_buf[crc_len] = msp_serial_checksum_buf(0, hdr_buf + V1_CHECKSUM_STARTPOS, hdr_len - V1_CHECKSUM_STARTPOS);
     125           0 :         crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len);
     126           0 :         crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], crc_buf, crc_len);
     127           0 :         crc_len++;
     128           0 :     } else if (msp_version == MSP_V2_NATIVE) {
     129           0 :         msp_header_v2_t * hdr_v2 = (msp_header_v2_t *)&hdr_buf[hdr_len];
     130           0 :         hdr_len += sizeof(msp_header_v2_t);
     131             : 
     132           0 :         hdr_v2->flags = packet->flags;
     133           0 :         hdr_v2->cmd = packet->cmd;
     134           0 :         hdr_v2->size = data_len;
     135             : 
     136           0 :         crc_buf[crc_len] = crc8_dvb_s2_update(0, (uint8_t *)hdr_v2, sizeof(msp_header_v2_t));
     137           0 :         crc_buf[crc_len] = crc8_dvb_s2_update(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len);
     138           0 :         crc_len++;
     139             :     } else {
     140             :         // Shouldn't get here
     141           0 :         return 0;
     142             :     }
     143             : 
     144             :     // Send the frame
     145       21972 :     return msp_serial_send_frame(msp, hdr_buf, hdr_len, sbuf_ptr(&packet->buf), data_len, crc_buf, crc_len);
     146             : }
     147             : 
     148             : /*
     149             :  ported from betaflight/src/main/msp/msp_serial.c
     150             :  */
     151        8116 : bool MSP::msp_parse_received_data(msp_port_t *msp, uint8_t c)
     152             : {
     153        8116 :     switch (msp->c_state) {
     154        2766 :     default:
     155             :     case MSP_IDLE:      // Waiting for '$' character
     156        2766 :         if (c == '$') {
     157         224 :             msp->msp_version = MSP_V1;
     158         224 :             msp->c_state = MSP_HEADER_START;
     159             :         } else {
     160        2542 :             return false;
     161             :         }
     162         224 :         break;
     163             : 
     164         224 :     case MSP_HEADER_START:  // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
     165         224 :         switch (c) {
     166         141 :         case 'M':
     167         141 :             msp->c_state = MSP_HEADER_M;
     168         141 :             break;
     169          76 :         case 'X':
     170          76 :             msp->c_state = MSP_HEADER_X;
     171          76 :             break;
     172           7 :         default:
     173           7 :             msp->c_state = MSP_IDLE;
     174           7 :             break;
     175             :         }
     176         224 :         break;
     177             : 
     178         141 :     case MSP_HEADER_M:      // Waiting for '<'
     179         141 :         if (c == '<') {
     180         141 :             msp->offset = 0;
     181         141 :             msp->checksum1 = 0;
     182         141 :             msp->checksum2 = 0;
     183         141 :             msp->c_state = MSP_HEADER_V1;
     184             :         } else {
     185           0 :             msp->c_state = MSP_IDLE;
     186             :         }
     187         141 :         break;
     188             : 
     189          76 :     case MSP_HEADER_X:
     190          76 :         if (c == '<') {
     191          76 :             msp->offset = 0;
     192          76 :             msp->checksum2 = 0;
     193          76 :             msp->msp_version = MSP_V2_NATIVE;
     194          76 :             msp->c_state = MSP_HEADER_V2_NATIVE;
     195             :         } else {
     196           0 :             msp->c_state = MSP_IDLE;
     197             :         }
     198          76 :         break;
     199             : 
     200         282 :     case MSP_HEADER_V1:     // Now receive v1 header (size/cmd), this is already checksummable
     201         282 :         msp->in_buf[msp->offset++] = c;
     202         282 :         msp->checksum1 ^= c;
     203         282 :         if (msp->offset == sizeof(msp_header_v1_t)) {
     204         141 :             msp_header_v1_t * hdr = (msp_header_v1_t *)&msp->in_buf[0];
     205             :             // Check incoming buffer size limit
     206         141 :             if (hdr->size > MSP_PORT_INBUF_SIZE) {
     207           0 :                 msp->c_state = MSP_IDLE;
     208         141 :             } else if (hdr->cmd == MSP_V2_FRAME_ID) {
     209             :                 // MSPv1 payload must be big enough to hold V2 header + extra checksum
     210           0 :                 if (hdr->size >= sizeof(msp_header_v2_t) + 1) {
     211           0 :                     msp->msp_version = MSP_V2_OVER_V1;
     212           0 :                     msp->c_state = MSP_HEADER_V2_OVER_V1;
     213             :                 } else {
     214           0 :                     msp->c_state = MSP_IDLE;
     215             :                 }
     216             :             } else {
     217         141 :                 msp->data_size = hdr->size;
     218         141 :                 msp->cmd_msp = hdr->cmd;
     219         141 :                 msp->cmd_flags = 0;
     220         141 :                 msp->offset = 0;                // re-use buffer
     221         141 :                 msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1;    // If no payload - jump to checksum byte
     222             :             }
     223             :         }
     224         282 :         break;
     225             : 
     226          78 :     case MSP_PAYLOAD_V1:
     227          78 :         msp->in_buf[msp->offset++] = c;
     228          78 :         msp->checksum1 ^= c;
     229          78 :         if (msp->offset == msp->data_size) {
     230          16 :             msp->c_state = MSP_CHECKSUM_V1;
     231             :         }
     232          78 :         break;
     233             : 
     234         141 :     case MSP_CHECKSUM_V1:
     235         141 :         if (msp->checksum1 == c) {
     236         141 :             msp->c_state = MSP_COMMAND_RECEIVED;
     237             :         } else {
     238           0 :             msp->c_state = MSP_IDLE;
     239             :         }
     240         141 :         break;
     241             : 
     242           0 :     case MSP_HEADER_V2_OVER_V1:     // V2 header is part of V1 payload - we need to calculate both checksums now
     243           0 :         msp->in_buf[msp->offset++] = c;
     244           0 :         msp->checksum1 ^= c;
     245           0 :         msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
     246           0 :         if (msp->offset == (sizeof(msp_header_v2_t) + sizeof(msp_header_v1_t))) {
     247           0 :             msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[sizeof(msp_header_v1_t)];
     248           0 :             msp->data_size = hdrv2->size;
     249             : 
     250             :             // Check for potential buffer overflow
     251           0 :             if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
     252           0 :                 msp->c_state = MSP_IDLE;
     253             :             } else {
     254           0 :                 msp->cmd_msp = hdrv2->cmd;
     255           0 :                 msp->cmd_flags = hdrv2->flags;
     256           0 :                 msp->offset = 0;                // re-use buffer
     257           0 :                 msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
     258             :             }
     259             :         }
     260           0 :         break;
     261             : 
     262           0 :     case MSP_PAYLOAD_V2_OVER_V1:
     263           0 :         msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
     264           0 :         msp->checksum1 ^= c;
     265           0 :         msp->in_buf[msp->offset++] = c;
     266             : 
     267           0 :         if (msp->offset == msp->data_size) {
     268           0 :             msp->c_state = MSP_CHECKSUM_V2_OVER_V1;
     269             :         }
     270           0 :         break;
     271             : 
     272           0 :     case MSP_CHECKSUM_V2_OVER_V1:
     273           0 :         msp->checksum1 ^= c;
     274           0 :         if (msp->checksum2 == c) {
     275           0 :             msp->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
     276             :         } else {
     277           0 :             msp->c_state = MSP_IDLE;
     278             :         }
     279           0 :         break;
     280             : 
     281         380 :     case MSP_HEADER_V2_NATIVE:
     282         380 :         msp->in_buf[msp->offset++] = c;
     283         380 :         msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
     284         380 :         if (msp->offset == sizeof(msp_header_v2_t)) {
     285          76 :             msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[0];
     286             : 
     287             :             // Check for potential buffer overflow
     288          76 :             if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
     289           0 :                 msp->c_state = MSP_IDLE;
     290             :             } else {
     291          76 :                 msp->data_size = hdrv2->size;
     292          76 :                 msp->cmd_msp = hdrv2->cmd;
     293          76 :                 msp->cmd_flags = hdrv2->flags;
     294          76 :                 msp->offset = 0;                // re-use buffer
     295          76 :                 msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
     296             :             }
     297             :         }
     298         380 :         break;
     299             : 
     300        3952 :     case MSP_PAYLOAD_V2_NATIVE:
     301        3952 :         msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
     302        3952 :         msp->in_buf[msp->offset++] = c;
     303             : 
     304        3952 :         if (msp->offset == msp->data_size) {
     305          76 :             msp->c_state = MSP_CHECKSUM_V2_NATIVE;
     306             :         }
     307        3952 :         break;
     308             : 
     309          76 :     case MSP_CHECKSUM_V2_NATIVE:
     310          76 :         if (msp->checksum2 == c) {
     311          76 :             msp->c_state = MSP_COMMAND_RECEIVED;
     312             :         } else {
     313           0 :             msp->c_state = MSP_IDLE;
     314             :         }
     315          76 :         break;
     316             :     }
     317        5574 :     return true;
     318             : }
     319             : 
     320             : #endif //HAL_MSP_ENABLED