betaflight/src/main/msp/msp_serial.c

/*
 * This file is part of Cleanflight.
 *
 * Cleanflight is free software: you can redistribute it and/or modify
 * it 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 is distributed in the hope that it 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include "platform.h"

#include "common/utils.h"

#include "drivers/buf_writer.h"
#include "drivers/serial.h"

#include "fc/runtime_config.h"

#include "io/serial.h"

#include "msp/msp.h"
#include "msp/msp_serial.h"


static mspProcessCommandFnPtr mspProcessCommandFn;
static mspPushCommandFnPtr mspPushCommandFn;
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
bufWriter_t *writer;


static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
{
    memset(mspPortToReset, 0, sizeof(mspPort_t));

    mspPortToReset->port = serialPort;
}

void mspSerialAllocatePorts(void)
{
    uint8_t portIndex = 0;
    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP);
    while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
        mspPort_t *mspPort = &mspPorts[portIndex];
        if (mspPort->port) {
            portIndex++;
            continue;
        }

        serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
        if (serialPort) {
            resetMspPort(mspPort, serialPort);
            portIndex++;
        }

        portConfig = findNextSerialPortConfig(FUNCTION_MSP);
    }
}

void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
{
    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
        mspPort_t *candidateMspPort = &mspPorts[portIndex];
        if (candidateMspPort->port == serialPort) {
            closeSerialPort(serialPort);
            memset(candidateMspPort, 0, sizeof(mspPort_t));
        }
    }
}

bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
{
    if (mspPort->c_state == MSP_IDLE) {
        if (c == '$') {
            mspPort->c_state = MSP_HEADER_START;
        } else {
            return false;
        }
    } else if (mspPort->c_state == MSP_HEADER_START) {
        mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
    } else if (mspPort->c_state == MSP_HEADER_M) {
        mspPort->c_state = (c == '<') ? MSP_HEADER_ARROW : MSP_IDLE;
    } else if (mspPort->c_state == MSP_HEADER_ARROW) {
        if (c > MSP_PORT_INBUF_SIZE) {
            mspPort->c_state = MSP_IDLE;

        } else {
            mspPort->dataSize = c;
            mspPort->offset = 0;
            mspPort->checksum = 0;
            mspPort->indRX = 0;
            mspPort->checksum ^= c;
            mspPort->c_state = MSP_HEADER_SIZE;
        }
    } else if (mspPort->c_state == MSP_HEADER_SIZE) {
        mspPort->cmdMSP = c;
        mspPort->checksum ^= c;
        mspPort->c_state = MSP_HEADER_CMD;
    } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset < mspPort->dataSize) {
        mspPort->checksum ^= c;
        mspPort->inBuf[mspPort->offset++] = c;
    } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset >= mspPort->dataSize) {
        if (mspPort->checksum == c) {
            mspPort->c_state = MSP_COMMAND_RECEIVED;
        } else {
            mspPort->c_state = MSP_IDLE;
        }
    }
    return true;
}

static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort)
{
    mspPostProcessFnPtr mspPostProcessFn = NULL;
    mspProcessCommandFn(mspPort, &mspPostProcessFn);

    mspPort->c_state = MSP_IDLE;
    return mspPostProcessFn;
}

/*
 * Process MSP commands from serial ports configured as MSP ports.
 *
 * Called periodically by the scheduler.
 */
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
{
    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
        mspPort_t * const mspPort = &mspPorts[portIndex];
        if (!mspPort->port) {
            continue;
        }

        // Big enough to fit a MSP_STATUS in one write.
        uint8_t buf[sizeof(bufWriter_t) + 20];
        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);

        mspPostProcessFnPtr mspPostProcessFn = NULL;
        while (serialRxBytesWaiting(mspPort->port)) {

            const uint8_t c = serialRead(mspPort->port);
            const bool consumed = mspSerialProcessReceivedData(mspPort, c);

            if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
                serialEvaluateNonMspData(mspPort->port, c);
            }

            if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
                mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort);
                break; // process one command at a time so as not to block.
            }
        }

        bufWriterFlush(writer);

        if (mspPostProcessFn) {
            waitForSerialPortToFinishTransmitting(mspPort->port);
            mspPostProcessFn(mspPort->port);
        }
    }
}

void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
{
    mspProcessCommandFn = mspProcessCommandFnToUse;
    memset(mspPorts, 0, sizeof(mspPorts));
    mspSerialAllocatePorts();
}

void mspSerialPush(uint8_t cmd, uint8_t *data, int buflen)
{
    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
        mspPort_t * const mspPort = &mspPorts[portIndex];
        if (!mspPort->port) {
            continue;
        }

        // Big enough for a OSD line
        uint8_t buf[sizeof(bufWriter_t) + 30];

        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);

        mspPushCommandFn(mspPort, cmd, data, buflen);

        bufWriterFlush(writer);
    }
}

void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFnToUse)
{
    mspPushCommandFn = mspPushCommandFnToUse;
}