File content as of revision 0:6b1f6139fb25:

#include "IsoTpHandler.h"

enum IsoTpMessageType
{
    SINGLE_FRAME = 0,      ///<  The single frame transferred contains the complete payload of up to 7 bytes (normal addressing) or 6 bytes (extended addressing)
    FIRST_FRAME = 1,       ///<  The first frame of a longer multi-frame message packet, used when more than 6/7 bytes of data segmented must be communicated. The first frame contains the length of the full packet, along with the initial data.
    CONSECUTIVE_FRAME = 2, ///<  A frame containing subsequent data for a multi-frame packet
    FLOW_CONTOL_FRAME = 3, ///<  The response from the receiver, acknowledging a First-frame segment. It lays down the parameters for the transmission of further consecutive frames.
};

enum IsoTpFlowType
{
    CLEAR_TO_SEND = 0,
    WAIT = 1,
    OVERFLOW_ABORT = 2,
};

const IsoTpHandler::IdleState IsoTpHandler::idleState;
const IsoTpHandler::ConsequtiveTransferState IsoTpHandler::consequtiveTransferState;

IsoTpHandler::IdleState::IdleState()
{
}

void IsoTpHandler::IdleState::processInput(const CANMessage* message, IsoTpHandler* context) const
{        
    if (!IsoTpHandler::isValidIsoTpPacket(message))
    {
        return;
    }
    uint8_t messageType = message->data[0] >> 4;
    if (messageType == SINGLE_FRAME)
    {
        uint8_t messageSize = message->data[0] & 0x0F;
        if (messageSize > message->len - 1)
        {
            printf("Iso tp message is too short: iso len %d vs can len %d\n", messageSize, message->len);
            return;
        }
        context->handle_decoded_packet(&message->data[1], messageSize);
        return;
    }
    if (messageType == FIRST_FRAME)
    {
        if (message->len != 8)
        {
            printf("Invalid iso tp message length for FIRST_FRAME, length is %d\n", message->len);
            return;
        }
        
        uint16_t messageSize = ((message->data[0] & 0x0F) << 8) | (message->data[1]);
        context->init_consequtive_reading(messageSize, &message->data[2]);
        context->setState(&consequtiveTransferState);
        return;
    }
    if (messageType == CONSECUTIVE_FRAME)
    {
        printf("Invalid iso tp message in idle state, because unexpected CONSECUTIVE_FRAME received\n");
        return;
    }
    if (messageType == FLOW_CONTOL_FRAME)
    {
        printf("Invalid iso tp message, because unexpected FLOW_CONTOL_FRAME received\n");
        return;
    }
    
    printf("Invalid iso tp message ?!\n");
}

void IsoTpHandler::IdleState::onEnter(IsoTpHandler* context) const
{
}

void IsoTpHandler::IdleState::onLeave(IsoTpHandler* context) const
{
}

IsoTpHandler::ConsequtiveTransferState::ConsequtiveTransferState()
{
}

void IsoTpHandler::ConsequtiveTransferState::processInput(const CANMessage* message, IsoTpHandler* context) const
{
     if (!IsoTpHandler::isValidIsoTpPacket(message))
    {
        return;
    }
    uint8_t messageType = message->data[0] >> 4;
    if (messageType == SINGLE_FRAME)
    {
            printf("Received SINGLE_FRAME, expected consequitve frame\n");
            return;
    }
    if (messageType == FIRST_FRAME)
    {
        printf("Received FIRST_FRAME, expected consequitve frame\n");
        return;
    }
    if (messageType == CONSECUTIVE_FRAME)
    {
        uint8_t index = message->data[0] & 0x0F;
        if (index != context->getExpectedIndex())
        {
            printf("In consequiive frame, received index %d, expected %d\n", index, context->getExpectedIndex());
            context->setState(&IsoTpHandler::idleState);
            return;
    
        }
        
        if (context->appendReceivedData(&message->data[1], message->len - 1))
        {
            printf("In consequtive frame, change state\n");
            
            context->setState(&IsoTpHandler::idleState);
        }
        return;
    }
    if (messageType == FLOW_CONTOL_FRAME)
    {
        printf("Received FLOW_CONTROL_FRAME, expected consequitve frame\n");
        return;
    }
    
    printf("Invalid iso tp message, expected consequitve frame ?!\n");
}

void IsoTpHandler::ConsequtiveTransferState::onEnter(IsoTpHandler* context) const
{
}

void IsoTpHandler::ConsequtiveTransferState::onLeave(IsoTpHandler* context) const
{
}
    
IsoTpHandler::IsoTpHandler(CAN* canInterface)
: m_state(&idleState)
, m_canInterface (canInterface)
{
    m_state->onEnter(this);
}

void IsoTpHandler::processCanMessage(const CANMessage* message)
{
    printf("Received new CAN message:\n");
    printf(" ID: 0x%X\n", message->id);
    printf(" Len: %d\n", message->len);
    printf(" Type: %s\n", (message->type == CANData ? "data" : "remote"));
    printf(" Format: %s\n", (message->format == CANStandard ? "standard" : "extended"));
    printf( "Data: ");
    if (message->len > 8) {
        //paranoia
        error(" WRONG DATA LEN! ");
        return;
    }
    for (unsigned int i = 0; i < message->len; ++i) {
        printf("%X ", message->data[i]);
    }
    printf("\n");
    m_state->processInput(message, this);
}

void IsoTpHandler::handle_decoded_packet(const uint8_t* data, uint16_t length)
{
    //todo write into mailbox so another thread can consume this or directly call a callback
    printf("New decoded packet: Length: %d\n", length);
    printf(" Data: ");
    for (uint16_t i = 0; i < length; ++i)
    {
        printf("%X ", data[i]);
    }
    printf("\n");
}

void IsoTpHandler::init_consequtive_reading(uint16_t messageSize, const uint8_t* data)
{
    char msgContent[8];
    msgContent[0] = (FLOW_CONTOL_FRAME << 4) | CLEAR_TO_SEND;
    msgContent[1] = 0; //remaining frames should to be sent without flow control or delay
    msgContent[2] = 0; //Separation Time (ST), minimum delay time between frames (end of one frame and the beginning of the other)
                       //<= 127, separation time in milliseconds.
                       //0xF1 to 0xF9, 100 to 900 microseconds.
    msgContent[3] = 0;
    msgContent[4] = 0;
    msgContent[5] = 0;
    msgContent[6] = 0;
    msgContent[7] = 0;
    m_canInterface->write(CANMessage(0x7DF, msgContent, sizeof(msgContent)));
    
    memcpy(m_messageBuffer, data, 6);
    m_expectedMessageSize = messageSize;
    m_currentMessageSize = 6;
    m_expectedIndex = 1;
}

void IsoTpHandler::setState(const State* state)
{
    if (state == m_state)
    {
        return;
    }
    
    m_state->onLeave(this);
    m_state = state;
    m_state->onEnter(this);
}

bool IsoTpHandler::isValidIsoTpPacket(const CANMessage* message)
{
    if (message->len < 1)
    {
        printf("Invalid iso tp message, length is zero\n");
        return false;
    } 
    uint8_t messageType = message->data[0] >> 4;
    if (messageType > FLOW_CONTOL_FRAME)
    {
        printf("Invalid iso tp message type %d\n", messageType);
        return false;
    }
    return true;
}

uint8_t IsoTpHandler::getExpectedIndex() const
{
    return m_expectedIndex;
}

void IsoTpHandler::incrementExpectedIndex()
{
    ++m_expectedIndex;
    if (m_expectedIndex >= 16)
    {
        m_expectedIndex = 0;
    }
}

bool IsoTpHandler::appendReceivedData(const uint8_t* data, uint8_t length)
{
    if (sizeof(m_messageBuffer) < m_currentMessageSize + length)
    {
        printf("Buffer in appendReceivedData too small, already got %d bytes, new %d bytes, expected %d bytes.\n", m_currentMessageSize, length, m_expectedMessageSize);
        return true; //switch state
    }
    
    if (m_expectedMessageSize < m_currentMessageSize + length)
    {
        printf("Got too much data in appendReceivedData, already got %d bytes, new %d bytes, expected %d bytes.\n", m_currentMessageSize, length, m_expectedMessageSize);
        length = m_expectedMessageSize - m_currentMessageSize;
    }
    
    memcpy(m_messageBuffer + m_currentMessageSize, data, length);
    m_currentMessageSize += length;
    
    if (m_expectedMessageSize == m_currentMessageSize)
    {
        handle_decoded_packet(m_messageBuffer, m_expectedMessageSize);
        return true; //switch state
    }
    
    return false; //do not switch state
}