LibPktHandler.cpp

//------------------------------------------------------------------------------
//This file is part of embKernel. 
//See license.txt for the full license governing this code.
//------------------------------------------------------------------------------

#include "LibPktHandler.hpp"
#include <stdlib.h>

LibPktHandler::LibPktHandler() {
    mCurrentPkt = 0;
    mCurrentPosition = 0;
    mIsError = false;
}

LibPktHandler::RESULT LibPktHandler::recvPkt(LibStreamIn& stream, PKT** pkt, Rtos::TICK timeout) {
    *pkt = 0;

    while (true) {
        if (mCurrentPosition == 0) {
            uint8_t value;
            if (stream.read(&value, 1, timeout) > 0) {
                mCmd = value;
                mCurrentPosition = 1;
            }
            else {
                return RES_TIMEOUT;
            }
        }
        else if (mCurrentPosition == 1) {
            uint8_t value;
            if (stream.read(&value, 1, timeout) > 0) {
                mCmd += ((uint16_t) value) << 8;
                mCurrentPosition = 2;
            }
            else {
                return RES_TIMEOUT;
            }
        }
        else if (mCurrentPosition == 2) {
            uint8_t value;
            if (stream.read(&value, 1, timeout) > 0) {
                mDataLen = value;
                mCurrentPosition = 3;
            }
            else {
                return RES_TIMEOUT;
            }
        }
        else if (mCurrentPosition == 3) {
            uint8_t value;
            if (stream.read(&value, 1, timeout) > 0) {
                mDataLen += ((uint16_t) value) << 8;
                mCurrentPkt = (PKT*) malloc(mDataLen + 4);
                if (mCurrentPkt) {
                    mCurrentPkt->hdr.cmd = mCmd;
                    mCurrentPkt->hdr.dataLen = mDataLen;

                }
                mCurrentPosition = 4;
            }
            else {
                return RES_TIMEOUT;
            }
        }
        else if (mCurrentPosition > 3) {
            if (mDataLen <= 0) {
                if (mCurrentPkt) {
                    *pkt = mCurrentPkt;
                    mCurrentPkt = 0;
                    mCurrentPosition = 0;
                    return RES_SUCCESS;
                }
                else {
                    return RES_PKT_DROPPED;
                }
            }
            int read;
            if (mCurrentPkt) {
                read = stream.read(&mCurrentPkt->data[mCurrentPosition - 4], mDataLen, timeout);
            }
            else {
                read = stream.skip(mDataLen, timeout);
            }
            if (read <= 0) {
                return RES_TIMEOUT;
            }
            mCurrentPosition += read;
            mDataLen -= read;
        }
    }
}

void LibPktHandler::freePkt(PKT** pkt) {
    if (*pkt != 0) {
        free(*pkt);
    }
}

LibPktHandler::RESULT LibPktHandler::sendPkt(LibStreamOut& stream, uint16_t cmd, uint16_t dataLen, const void* data, Rtos::TICK timeout) {
    PKT_HDR hdr = {
            cmd, dataLen };

    int written = stream.write(&hdr, 4, timeout);
    if (written != 4) {
        return RES_TIMEOUT;
    }
    written = stream.write(data, dataLen, timeout);
    if (written != dataLen) {
        return RES_TIMEOUT;
    }
    return RES_SUCCESS;
}

void LibPktHandler::reset() {
    mCurrentPosition = 0;
    freePkt(&mCurrentPkt);
    mIsError = false;
}