IsoTpHandler.cpp

00001 #include "IsoTpHandler.h"
00002 #include "PidDecoder.h"
00003 
00004 #include "MODSERIAL.h"
00005 extern MODSERIAL pc;
00006 
00007 #ifdef ACTIVATE_DEBUG_OUTPUT
00008 #define DEBUG_PRINT(format, ...) pc.printf(format, ##__VA_ARGS__) 
00009 #else
00010 #define DEBUG_PRINT(format, ...)
00011 #endif
00012 
00013 static PidDecoder pidDecoder;
00014 
00015 enum IsoTpMessageType
00016 {
00017     SINGLE_FRAME = 0,      ///<  The single frame transferred contains the complete payload of up to 7 bytes (normal addressing) or 6 bytes (extended addressing)
00018     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.
00019     CONSECUTIVE_FRAME = 2, ///<  A frame containing subsequent data for a multi-frame packet
00020     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.
00021 };
00022 
00023 enum IsoTpFlowType
00024 {
00025     CLEAR_TO_SEND = 0,
00026     WAIT = 1,
00027     OVERFLOW_ABORT = 2,
00028 };
00029 
00030 const IsoTpHandler::IdleState IsoTpHandler::idleState;
00031 const IsoTpHandler::ConsequtiveTransferState IsoTpHandler::consequtiveTransferState;
00032 
00033 IsoTpHandler::IdleState::IdleState()
00034 {
00035 }
00036 
00037 void IsoTpHandler::IdleState::processInput(const CANMessage* message, IsoTpHandler* context) const
00038 {
00039     if (!IsoTpHandler::isValidIsoTpPacket(message))
00040     {
00041         return;
00042     }
00043     uint8_t messageType = message->data[0] >> 4;
00044     if (messageType == SINGLE_FRAME)
00045     {
00046         uint8_t messageSize = message->data[0] & 0x0F;
00047         if (messageSize > message->len - 1)
00048         {
00049             DEBUG_PRINT("Iso tp message is too short: iso len %d vs can len %d\r\n", messageSize, message->len);
00050             return;
00051         }
00052         context->handle_decoded_packet(&message->data[1], messageSize);
00053         return;
00054     }
00055     if (messageType == FIRST_FRAME)
00056     {
00057         if (message->len != 8)
00058         {
00059             DEBUG_PRINT("Invalid iso tp message length for FIRST_FRAME, length is %d\r\n", message->len);
00060             return;
00061         }
00062         
00063         uint16_t messageSize = ((message->data[0] & 0x0F) << 8) | (message->data[1]);
00064         context->init_consequtive_reading (messageSize, &message->data[2]);
00065         context->setState(&consequtiveTransferState);
00066         return;
00067     }
00068     if (messageType == CONSECUTIVE_FRAME)
00069     {
00070         DEBUG_PRINT("Invalid iso tp message in idle state, because unexpected CONSECUTIVE_FRAME received\r\n");
00071         return;
00072     }
00073     if (messageType == FLOW_CONTOL_FRAME)
00074     {
00075         DEBUG_PRINT("Invalid iso tp message, because unexpected FLOW_CONTOL_FRAME received\r\n");
00076         return;
00077     }
00078     
00079     DEBUG_PRINT("Invalid iso tp message ?!\r\n");
00080 }
00081 
00082 void IsoTpHandler::IdleState::onEnter(IsoTpHandler* context) const
00083 {
00084 }
00085 
00086 void IsoTpHandler::IdleState::onLeave(IsoTpHandler* context) const
00087 {
00088 }
00089 
00090 IsoTpHandler::ConsequtiveTransferState::ConsequtiveTransferState()
00091 {
00092 }
00093 
00094 void IsoTpHandler::ConsequtiveTransferState::processInput(const CANMessage* message, IsoTpHandler* context) const
00095 {
00096      if (!IsoTpHandler::isValidIsoTpPacket(message))
00097     {
00098         return;
00099     }
00100     uint8_t messageType = message->data[0] >> 4;
00101     if (messageType == SINGLE_FRAME)
00102     {
00103             DEBUG_PRINT("Received SINGLE_FRAME, expected consequitve frame\r\n");
00104             return;
00105     }
00106     if (messageType == FIRST_FRAME)
00107     {
00108         DEBUG_PRINT("Received FIRST_FRAME, expected consequitve frame\r\n");
00109         return;
00110     }
00111     if (messageType == CONSECUTIVE_FRAME)
00112     {
00113         uint8_t index = message->data[0] & 0x0F;
00114         if (index != context->getExpectedIndex())
00115         {
00116             DEBUG_PRINT("In consequiive frame, received index %d, expected %d\r\n", index, context->getExpectedIndex());
00117             context->setState(&IsoTpHandler::idleState);
00118             return;
00119     
00120         }
00121         
00122         if (context->appendReceivedData(&message->data[1], message->len - 1))
00123         {
00124             DEBUG_PRINT("In consequtive frame, change state\r\n");
00125             
00126             context->setState(&IsoTpHandler::idleState);
00127         }
00128         context->incrementExpectedIndex();
00129         return;
00130     }
00131     if (messageType == FLOW_CONTOL_FRAME)
00132     {
00133         DEBUG_PRINT("Received FLOW_CONTROL_FRAME, expected consequitve frame\r\n");
00134         return;
00135     }
00136     
00137     DEBUG_PRINT("Invalid iso tp message, expected consequitve frame ?!\r\n");
00138 }
00139 
00140 void IsoTpHandler::ConsequtiveTransferState::onEnter(IsoTpHandler* context) const
00141 {
00142 }
00143 
00144 void IsoTpHandler::ConsequtiveTransferState::onLeave(IsoTpHandler* context) const
00145 {
00146 }
00147     
00148 IsoTpHandler::IsoTpHandler(CAN* canInterface)
00149 : m_state(&idleState)
00150 , m_canInterface (canInterface)
00151 {
00152     m_state->onEnter(this);
00153 }
00154 
00155 void IsoTpHandler::processCanMessage(const CANMessage* message)
00156 {
00157     DEBUG_PRINT("Received new CAN message:\r\n");
00158     DEBUG_PRINT(" ID: 0x%X\r\n", message->id);
00159     DEBUG_PRINT(" Len: %d\r\n", message->len);
00160     DEBUG_PRINT(" Type: %s\r\n", (message->type == CANData ? "data" : "remote"));
00161     DEBUG_PRINT(" Format: %s\r\n", (message->format == CANStandard ? "standard" : "extended"));
00162     DEBUG_PRINT( "Data: ");
00163     if (message->len > 8) {
00164         //paranoia
00165         error(" WRONG DATA LEN! ");
00166         return;
00167     }
00168     for (unsigned int i = 0; i < message->len; ++i) {
00169         DEBUG_PRINT("%X ", message->data[i]);
00170     }
00171     DEBUG_PRINT("\r\n");
00172     m_state->processInput(message, this);
00173 }
00174 
00175 void IsoTpHandler::handle_decoded_packet(const uint8_t* data, uint16_t length)
00176 {
00177     //todo write into mailbox so another thread can consume this or directly call a callback
00178     DEBUG_PRINT("New decoded packet: Length: %d\r\n", length);
00179     DEBUG_PRINT(" Data: ");
00180     for (uint16_t i = 0; i < length; ++i)
00181     {
00182         DEBUG_PRINT("%X ", data[i]);
00183     }
00184     DEBUG_PRINT("\r\n");
00185     
00186     pidDecoder.decode(data, length);
00187 }
00188 
00189 void IsoTpHandler::init_consequtive_reading (uint16_t messageSize, const uint8_t* data)
00190 {
00191     char msgContent[8];
00192     msgContent[0] = (FLOW_CONTOL_FRAME << 4) | CLEAR_TO_SEND;
00193     msgContent[1] = 0; //remaining frames should to be sent without flow control or delay
00194     msgContent[2] = 0; //Separation Time (ST), minimum delay time between frames (end of one frame and the beginning of the other)
00195                        //<= 127, separation time in milliseconds.
00196                        //0xF1 to 0xF9, 100 to 900 microseconds.
00197     msgContent[3] = 0;
00198     msgContent[4] = 0;
00199     msgContent[5] = 0;
00200     msgContent[6] = 0;
00201     msgContent[7] = 0;
00202     m_canInterface->write(CANMessage(0x7E0, msgContent, sizeof(msgContent)));  //or 7DF?
00203     
00204     memcpy(m_messageBuffer, data, 6);
00205     m_expectedMessageSize = messageSize;
00206     m_currentMessageSize = 6;
00207     m_expectedIndex = 1;
00208 }
00209 
00210 void IsoTpHandler::setState(const State* state)
00211 {
00212     if (state == m_state)
00213     {
00214         return;
00215     }
00216     
00217     m_state->onLeave(this);
00218     m_state = state;
00219     m_state->onEnter(this);
00220 }
00221 
00222 bool IsoTpHandler::isValidIsoTpPacket(const CANMessage* message)
00223 {
00224     if (message->len < 1)
00225     {
00226         DEBUG_PRINT("Invalid iso tp message, length is zero\r\n");
00227         return false;
00228     } 
00229     uint8_t messageType = message->data[0] >> 4;
00230     if (messageType > FLOW_CONTOL_FRAME)
00231     {
00232         DEBUG_PRINT("Invalid iso tp message type %d\r\n", messageType);
00233         return false;
00234     }
00235     return true;
00236 }
00237 
00238 uint8_t IsoTpHandler::getExpectedIndex() const
00239 {
00240     return m_expectedIndex;
00241 }
00242 
00243 void IsoTpHandler::incrementExpectedIndex()
00244 {
00245     ++m_expectedIndex;
00246     if (m_expectedIndex >= 16)
00247     {
00248         m_expectedIndex = 0;
00249     }
00250 }
00251 
00252 bool IsoTpHandler::appendReceivedData(const uint8_t* data, uint8_t length)
00253 {
00254     if (sizeof(m_messageBuffer) < m_currentMessageSize + length)
00255     {
00256         DEBUG_PRINT("Buffer in appendReceivedData too small, already got %d bytes, new %d bytes, expected %d bytes.\r\n", m_currentMessageSize, length, m_expectedMessageSize);
00257         return true; //switch state
00258     }
00259     
00260     if (m_expectedMessageSize < m_currentMessageSize + length)
00261     {
00262         DEBUG_PRINT("Got too much data in appendReceivedData, already got %d bytes, new %d bytes, expected %d bytes.\r\n", m_currentMessageSize, length, m_expectedMessageSize);
00263         length = m_expectedMessageSize - m_currentMessageSize;
00264     }
00265     
00266     memcpy(m_messageBuffer + m_currentMessageSize, data, length);
00267     m_currentMessageSize += length;
00268     
00269     if (m_expectedMessageSize == m_currentMessageSize)
00270     {
00271         handle_decoded_packet(m_messageBuffer, m_expectedMessageSize);
00272         return true; //switch state
00273     }
00274     
00275     return false; //do not switch state
00276 }