File content as of revision 0:21e4e3afa5e6:

#include "SerialInterfaceProtocol.h"

// #define DEBUG_MESSAGE

SerialInterfaceProtocol::SerialInterfaceProtocol(SerialBuffer_t *in, SerialBuffer_t *out)
{
    // assign input and output buffers
    SerialInputBuffer = in;
    SerialOutputBuffer = out;
    
    // init command vector table
    for (int i = 0; i < SIP_CMD_VECTOR_TABLE_SZ; i++)
    {
        CommandVectorTable[i] = NULL;
    }
    
    // init internal state machine
    state = CommandPacket::NONE;
    
    // init internal state
    isChecksumEnabled = true;
}

SerialInterfaceProtocol::~SerialInterfaceProtocol()
{
    
}

void SerialInterfaceProtocol::registerCommand(uint8_t command, callback_func f)
{
    CommandVectorTable[command] = f;
}

void SerialInterfaceProtocol::deRegisterCommand(uint8_t command)
{
    CommandVectorTable[command] = NULL;
}

int SerialInterfaceProtocol::execute(uint8_t *response, uint8_t *response_length)
{
    
    // execute the command if it's already been registered
    if (CommandVectorTable[PacketBuffer.command] != NULL)
    {       
        return CommandVectorTable[PacketBuffer.command](
            PacketBuffer.payload, 
            PacketBuffer.length,
            response, 
            response_length
        );
    }
    
    printf("Callback function not registered\r\n");
    
    return -1;
}

int SerialInterfaceProtocol::assemble(uint8_t *response, uint8_t response_length)
{
    // prepare for packet buffer
    PacketBuffer.sflag = CommandPacket::CP_SFLAG;
    
    PacketBuffer.command = 0xE0 | ((uint8_t) PacketBuffer.errno & 0x0f);
    
    PacketBuffer.length = response_length;
    
    memcpy(PacketBuffer.payload, response, response_length);
    
    PacketBuffer.checksum = PacketBuffer.generate_checksum();
    
    PacketBuffer.eflag = CommandPacket::CP_EFLAG;

    
    // serialize the packet
    int total_len = PacketBuffer.length * 2 + 8;
    uint8_t buffer[total_len];
    memset(buffer, 0x0, total_len);
    
    PacketBuffer.serialize(buffer);
    
    // add to ring buffer
    for (int i = 0; i < total_len; i++)
    {
        SerialOutputBuffer->enqueue(buffer[i]);
    }
    
    return 0;
    
}

void SerialInterfaceProtocol::poll()
{
    static uint8_t payload_counter = 0;
    uint8_t response[SIP_MAX_RESP_LEN];
    uint8_t response_length;
    
    // fetch data from ring buffer
    while (SerialInputBuffer->getCounter() > 0)
    {
        uint8_t ch;
        ch = SerialInputBuffer->dequeue();
        
        // reset state to keep sync
        if (ch == CommandPacket::CP_SFLAG)
        {
            state = CommandPacket::SFLAG;
            
            // reset variable
            payload_counter = 0;
            memset(PacketBuffer.payload, 0x0, sizeof(PacketBuffer.payload));
            PacketBuffer.errno = CommandPacket::NO_ERROR;
        }
        
        switch (state)
        {
            case CommandPacket::SFLAG:
                PacketBuffer.sflag = ch;
                state = CommandPacket::COMMAND_H;
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::SFLAG: 0x%x\r\n", PacketBuffer.sflag);
#endif
                break;
                
            case CommandPacket::COMMAND_H:
                PacketBuffer.command = hexchar_to_uint8(ch) << 4;
                state = CommandPacket::COMMAND_L;
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::COMMAND_H: 0x%x\r\n", PacketBuffer.command);
#endif
                break;
                
            case CommandPacket::COMMAND_L:
                PacketBuffer.command |= (hexchar_to_uint8(ch) & 0x0f);
                state = CommandPacket::LENGTH_H;
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::COMMAND_L: 0x%x\r\n", PacketBuffer.command);
#endif
                break;
                
            case CommandPacket::LENGTH_H:
                PacketBuffer.length = hexchar_to_uint8(ch) << 4;
                state = CommandPacket::LENGTH_L;
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::LENGTH_H: 0x%x\r\n", PacketBuffer.length);
#endif
                break;
                
            case CommandPacket::LENGTH_L:
                PacketBuffer.length |= (hexchar_to_uint8(ch) & 0x0f);
                if (PacketBuffer.length != 0) // if the length is not zero, then proceed to payload state
                {
                    state = CommandPacket::PAYLOAD_H;
                }
                else // otherwise proceed to checksum state
                {
                    state = CommandPacket::CHECKSUM_H;
                }
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::LENGTH_L: 0x%x\r\n", PacketBuffer.length);
#endif
                break;
            
            case CommandPacket::PAYLOAD_H:
                PacketBuffer.payload[payload_counter] = hexchar_to_uint8(ch) << 4; // store higher 4 bits of payload
                state = CommandPacket::PAYLOAD_L;
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::PAYLOAD_H: 0x%x\r\n", PacketBuffer.payload[payload_counter]);
#endif
                break;
                
            case CommandPacket::PAYLOAD_L:
                PacketBuffer.payload[payload_counter++] |= (hexchar_to_uint8(ch) & 0x0f); // store lower 4 bits of payload
                if (payload_counter < PacketBuffer.length) // append ch to payload until reach the length
                {
                    state = CommandPacket::PAYLOAD_H;
                }
                else
                {
                    state = CommandPacket::CHECKSUM_H;
                }
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::PAYLOAD_L: 0x%x\r\n", PacketBuffer.payload[payload_counter - 1]);
#endif
                break;
                
            case CommandPacket::CHECKSUM_H:
                PacketBuffer.checksum = hexchar_to_uint8(ch) << 4;
                state = CommandPacket::CHECKSUM_L;
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::CHECKSUM_H: 0x%x\r\n", PacketBuffer.checksum);
#endif
                break;
                
            case CommandPacket::CHECKSUM_L:
                PacketBuffer.checksum |= (hexchar_to_uint8(ch) & 0x0f);
                
                // checksum can be turned off
                if (isChecksumEnabled)
                {
                    if (PacketBuffer.verify()) // checksum match
                    {
                        state = CommandPacket::EFLAG;
                    }
                    else // checksum mismatch
                    {
                        PacketBuffer.errno = CommandPacket::INVALID_CS_ERROR;
                        
                        // prepare for checksum error response
                        PacketBuffer.errno = CommandPacket::INVALID_CS_ERROR;
                        assemble(response, response_length);
                        
                        state = CommandPacket::NONE;
                    }
                }
                else
                {
                    state = CommandPacket::EFLAG;
                }
                
                
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::CHECKSUM_L: 0x%x\r\n", PacketBuffer.checksum);
#endif
                break;
                
            case CommandPacket::EFLAG:
                if (ch == CommandPacket::CP_EFLAG)
                {
                    PacketBuffer.eflag = ch;
                    
                    // clear response and response length
                    response_length = 0;
                    memset(response, 0x0, sizeof(response));
                    
                    // execute command
                    int ret = execute(response, &response_length);
                    
                    if (ret < 0) // command not registered
                    {
                        PacketBuffer.errno = CommandPacket::INVALID_CMD_ERROR;
                    }
                    else if (ret != 0) // error to execute
                    {
                        PacketBuffer.errno = CommandPacket::INVALID_EXEC_ERROR;
                    }
                    
                    assemble(response, response_length);
                }
                state = CommandPacket::NONE;
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::EFLAG: 0x%x\r\n", PacketBuffer.eflag);
#endif
                break;
                
            case CommandPacket::NONE:
                PacketBuffer.errno = CommandPacket::INVALID_SFLAG_ERROR;
                
                // TODO:: Execute error generator
                PacketBuffer.errno = CommandPacket::INVALID_SFLAG_ERROR;
                assemble(response, response_length);
                
#ifdef DEBUG_MESSAGE
                printf("CommandPacket::NONE\r\n");
#endif
                break;
                
            default:
                break;
        }
    }
}

void SerialInterfaceProtocol::disableChecksum()
{
    isChecksumEnabled = false;
}

void SerialInterfaceProtocol::enableChecksum()
{
    isChecksumEnabled = true;
}