Revision 0:6b1f6139fb25, committed 2014-04-22

Comitter:

chrta

Date:

Tue Apr 22 14:51:04 2014 +0000

Child:

1:ca506b88b1d6

Commit message:

Initial checkin

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IsoTpHandler.cpp	Tue Apr 22 14:51:04 2014 +0000
@@ -0,0 +1,261 @@
+#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
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IsoTpHandler.h	Tue Apr 22 14:51:04 2014 +0000
@@ -0,0 +1,85 @@
+#ifndef ISO_TP_HANDLER
+#define ISO_TP_HANDLER
+
+#include "mbed.h"
+
+/**
+ * http://en.wikipedia.org/wiki/ISO_15765-2
+ */
+class IsoTpHandler
+{
+private:
+    class State
+    {
+    public:
+        virtual ~State() {}
+        virtual void processInput(const CANMessage* message, IsoTpHandler* context) const = 0;
+        virtual void onEnter(IsoTpHandler* context) const = 0;
+        virtual void onLeave(IsoTpHandler* context) const = 0;
+    };
+    
+    /**
+     * No special packet expected 
+     */
+    class IdleState : public State
+    {
+    public:
+        IdleState();
+        
+        virtual void processInput(const CANMessage* message, IsoTpHandler* context) const;
+        virtual void onEnter(IsoTpHandler* context) const;
+        virtual void onLeave(IsoTpHandler* context) const;
+    };
+    
+    /**
+     * Expect packets of type "consecutive frame"
+     */
+    class ConsequtiveTransferState : public State
+    {
+    public:
+        ConsequtiveTransferState();
+        
+        virtual void processInput(const CANMessage* message, IsoTpHandler* context) const;
+        virtual void onEnter(IsoTpHandler* context) const;
+        virtual void onLeave(IsoTpHandler* context) const;
+    };
+public:
+    IsoTpHandler(CAN* canInterface);
+      
+    void processCanMessage(const CANMessage* message);
+    
+    void handle_decoded_packet(const uint8_t* data, uint16_t length);
+        
+    /**
+     *
+     * \param[in] data Always 6 bytes.
+     */
+    void init_consequtive_reading(uint16_t messageSize, const uint8_t* data);
+    uint8_t getExpectedIndex() const;
+    void incrementExpectedIndex();
+    /**
+     * \retval \c True if the state should be switched.
+     * \retval \c False if the state not change.
+     */
+    bool appendReceivedData(const uint8_t* data, uint8_t length);
+
+    void setState(const State* state);
+
+    static const IdleState idleState;
+    static const ConsequtiveTransferState consequtiveTransferState;
+    
+    static bool isValidIsoTpPacket(const CANMessage* message);
+
+private:
+    const State* m_state;
+    
+    CAN* m_canInterface;
+    
+    uint8_t m_messageBuffer[256];
+    uint16_t m_expectedMessageSize;
+    uint16_t m_currentMessageSize;
+    uint8_t m_expectedIndex;
+};
+    
+
+#endif //ISO_TP_HANDLER
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 22 14:51:04 2014 +0000
@@ -0,0 +1,56 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "IsoTpHandler.h"
+ 
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+CAN can2(p30, p29);
+IsoTpHandler tpHandler(&can2);
+ 
+void led2_thread(void const *args) {
+    while (true) {
+        led2 = !led2;
+        Thread::wait(1000);
+    }
+}
+
+Mail<CANMessage, 16> can_rx_queue;
+
+void can_process_packets(void const *args) {
+    while (true) {
+        osEvent evt = can_rx_queue.get(osWaitForever);
+        if (evt.status == osEventMail) {
+            CANMessage *msg = (CANMessage*) evt.value.p;
+            tpHandler.processCanMessage(msg);
+            can_rx_queue.free(msg);
+        }
+    }
+}
+
+ 
+void can_rx_int_handler() {
+    CANMessage* msg = can_rx_queue.alloc();
+    if (!can2.read(*msg))
+    {
+        //this should not happen, because this function is called from the rx interrupt
+        can_rx_queue.free(msg);
+        return;
+    }
+    
+    osStatus error_code = can_rx_queue.put(msg); 
+    if (error_code != osOK) {
+        error("Putting can message into mailbox failed with code %d!", error);
+    }
+}
+ 
+int main() {
+    can2.frequency(500000);
+    can2.attach(can_rx_int_handler);
+    Thread thread(led2_thread);
+    Thread can_thread(can_process_packets);
+    
+    while (true) {
+        led1 = !led1;
+        Thread::wait(500);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Apr 22 14:51:04 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#4ef72665e2c8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 22 14:51:04 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6473597d706e
\ No newline at end of file