Revision 0:cbd88c574455, committed 2012-12-11

Comitter:

foxdie

Date:

Tue Dec 11 21:31:57 2012 +0000

Commit message:

First attempt to send DIC message, known to not work at this stage.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 11 21:31:57 2012 +0000
@@ -0,0 +1,281 @@
+#include "mbed.h"
+#include <vector>
+#include <algorithm>
+#include <queue>
+#include <string>
+
+DigitalOut rx_led(LED1);
+DigitalOut tx_led(LED2);
+DigitalOut filter_led(LED3);
+DigitalOut notify_led(LED4);
+Serial pc(USBTX, USBRX);
+CAN gmlan(p30, p29);
+Timer uptime;
+Timeout notifyHandler;
+bool filterPackets = false;
+vector<int> filteredPackets;
+extern "C" void mbed_reset();
+
+namespace mbed {
+    class CANHeader {
+        // Example header packet for Steering Wheel Switches:
+        // Hexadecimal:    0x10     0x0D     0x00     0x60
+        // Binary:       00010000 00001101 00000000 01100000
+        // Priority:        ---
+        // Arbitration:        -- -------- ---
+        // Sending ECU:                       ----- --------
+
+        private:
+            int priorityID, arbitrationID, senderID;
+        
+        public:
+            int priority(void) { return priorityID; }
+            void priority(int _priority) { priorityID = _priority; }
+            int arbitration(void) { return arbitrationID; }
+            void arbitration(int _arbitration) { arbitrationID = _arbitration; }
+            int sender(void) { return senderID; }
+            void sender(int _sender) { senderID = _sender; }
+            
+            void decode(int _header) {
+                priorityID = (_header >> 26) & 0x7;
+                arbitrationID = (_header >> 13) & 0x1FFF;
+                senderID = (_header >> 0)  & 0x1FFF;
+            }
+            int encode(void) {
+                long int buffer = 0;
+                buffer = (buffer << 3) | 0x0; // 3 bit padding
+                buffer = (buffer << 3) | priorityID;
+                buffer = (buffer << 13) | arbitrationID;
+                buffer = (buffer << 13) | senderID;
+                return buffer;
+            }
+    };
+}
+
+void clearNotify()
+{
+    notify_led = 0;
+}
+void notifyUser()
+{
+    // Turn on notify LED and set timer to turn it off again shortly
+    notify_led = 1;
+    notifyHandler.detach();
+    notifyHandler.attach(&clearNotify, 0.1);
+    
+    // Set up notification header
+    CANHeader notifyHeader;
+    notifyHeader.priority(0x4);         // Medium priority
+    notifyHeader.arbitration(0x186);    // Text notification on top of cluster screen
+    notifyHeader.sender(0x81);          // Sent from an audio source
+    
+    // Define notification message
+    string message = "GMLAN pwned!";
+    int eachPacketLength = 8;
+    
+    // Initialise packet data
+    string packetData;
+    packetData.push_back(0x45);   // Starting packet for message
+    packetData.push_back(0x1);    // Multi-packet count, start at 1, we change it later
+    packetData.push_back(0x1);    // Arrow to display 01 = both, 81 = up, 41 = down
+    
+    // Read notification message into packet data vector and add terminator at the end
+    for (int i = 0; i < message.size(); i++)
+        packetData.push_back(message[i]);
+    packetData.push_back(0x4);    // Message terminator byte
+    
+    // Insert subsequent header every <eachPacketLength> bytes
+    int byteCount = 0;
+    int packetCount = 1;
+    div_t packetDivisor;    
+    while (byteCount < packetData.size())
+    {
+        packetDivisor = div(byteCount + 1, eachPacketLength);
+        if (packetDivisor.rem == 0 && byteCount + 1 < packetData.size())
+        {
+            packetCount++;
+            string::iterator pdIndex = packetData.begin() + byteCount;
+            packetData.insert(pdIndex + 1, 1, 0x44);
+            packetData.insert(pdIndex + 2, 1, packetCount);
+        }
+        byteCount++;;
+    }
+    packetData[1] = packetCount;
+    
+    // Make sure we have null-padding bytes towards the end
+    packetDivisor = div(packetData.size(), eachPacketLength);
+    int padding = eachPacketLength - packetDivisor.rem;
+    while (padding > 0)
+    {
+        packetData.push_back(0x0);
+        padding--;
+    }
+    
+    // Split packet list into groups of 8 bytes
+    char buffer [eachPacketLength];
+    for (int packetIndex = 0; packetIndex < packetCount; packetIndex++)// < packetData.size(); byte += eachPacketLength)
+    {
+        for (int i = 0; i < eachPacketLength; i++)
+            buffer[i] = packetData[(packetIndex*eachPacketLength)+i];
+        
+        CANMessage data = CANMessage(notifyHeader.encode(), buffer, eachPacketLength);
+        
+        pc.printf("Attemping to send [0x%08X] ", data.id);
+        pc.printf("[%02X", data.data[0]);  // Print first byte
+        for (int i = 1; i < data.len; i++)
+            pc.printf(" %02X", data.data[i]);  // Print additional byte(s) with a preceeding space
+        pc.printf("]\r\n");
+        
+        if (gmlan.write(data))
+            pc.printf("Message sent!\r\n");
+        pc.printf("Error count: TX=%u RX=%u\r\n", gmlan.tderror(), gmlan.rderror());
+    }
+}
+
+void processMessage()
+{
+    // Turn on rx_led to indicate we are receiving data
+    rx_led = 1;
+    
+    // Create a CANMessage object and read the buffer into it
+    CANMessage rxmsg;
+    gmlan.read(rxmsg);
+    
+    // Check to see if our header is in the filtered packet list or we are filtering, if so we skip over
+    bool packetFound = false;
+    if (find(filteredPackets.begin(), filteredPackets.end(), rxmsg.id) != filteredPackets.end())
+        packetFound = true;
+        
+    // If we are filtering packets and this one isn't found, add it to the list
+    if (filterPackets == true && packetFound == false)
+    {
+        filteredPackets.push_back(rxmsg.id);
+        pc.printf("Added 0x%08X to filter at position %u\r\n", rxmsg.id, filteredPackets.size());
+    }
+    else if (filterPackets == false && packetFound == false) {
+        CANHeader rxHeader;
+        rxHeader.decode(rxmsg.id);
+        
+        // Print these results to the serial terminal using printf to format
+        // Ref: http://www.cplusplus.com/reference/cstdio/printf/
+        pc.printf("[%10.2f] ", uptime.read());            // Seconds (to 2 decimal places) since start
+        pc.printf("[0x%1X] ", rxHeader.priority());       // Packet priority (0-7, 0 being highest)
+        pc.printf("[0x%4X] ", rxHeader.arbitration());    // Arbitration ID (0x0 to 0x1FFF)
+        pc.printf("[0x%4X] ", rxHeader.sender());         // ECU ID (0x0 to 0x1FFF)
+        pc.printf("[0x%08X] ", rxmsg.id);                 // Full header for legacy reasons (4 bytes)
+        pc.printf("[%d] ", rxmsg.len);                    // Length of message (0-8 bytes typically)
+        
+        // Process actual message here, only run this if we have a message
+        if (rxmsg.len > 0)
+        {
+            pc.printf("[%02X", rxmsg.data[0]);  // Print first byte
+            for (int i = 1; i < rxmsg.len; i++)
+            pc.printf(" %02X", rxmsg.data[i]);  // Print additional byte(s) with a preceeding space
+            pc.printf("]");                     // Print closing bracket
+        }
+        pc.printf("\r\n");                      // Print carriage return and newline
+        
+        // Try and do some clever stuff, see if we've got a particular packet and notify if we do
+        // In this example, we are testing to see if the steering wheel select button has been pressed
+        if (rxHeader.arbitration() == 0x68 && rxmsg.len > 0)
+        {
+            if (rxmsg.data[0] & 0x1 == 0x1)
+            {
+                notifyUser();
+            }
+        }
+    }
+        
+    // Turn off our rx_led as we have finished reading data
+    rx_led = 0;
+}
+
+void clearAndHome()
+{
+    pc.printf("%c", 27);    // ESC
+    pc.printf("[2J");       // clear screen
+    pc.printf("%c", 27);    // ESC
+    pc.printf("[H");        // cursor to home
+}
+
+int main() {
+    // Set serial baud rate to 115kbit
+    pc.baud(115200);
+    
+    // Set CANBUS to 33.3kbit and put into monitor mode (does not ACK packets, aka stealth mode)
+    int baudrate = 33333;
+    gmlan.frequency(baudrate);
+    gmlan.monitor(false);
+    
+    // Clear serial terminal
+    clearAndHome();
+    pc.printf("Keys:\r\n");
+    pc.printf("F = start/stop filter capture\r\n");
+    pc.printf("D = display filtered headers\r\n");
+    pc.printf("C = clear filter\r\n");
+    pc.printf("R = restart mbed\r\n");
+    pc.printf("T = send test packet\r\n");
+    pc.printf("Starting packet capture at %i bps\r\n\r\n", baudrate);
+    
+    // Start capturing packets
+    uptime.start();
+    gmlan.attach(&processMessage);
+    
+    while(1) {
+        // Poll serial for keypresses for certain tasks
+        if (pc.readable())
+        {
+            switch (pc.getc())
+            {
+                case 'f':
+                case 'F':
+                {
+                    // Toggle filter
+                    filterPackets = !filterPackets;
+                    filter_led = (int)filterPackets;
+                    break;
+                }
+                case 'c':
+                case 'C':
+                {
+                    // Clear filter
+                    filteredPackets.clear();
+                    pc.printf("Packet filter cleared\r\n");
+                    break;
+                }
+                case 'd':
+                case 'D':
+                {
+                    // Show filter
+                    clearAndHome();
+                    pc.printf("[%u] entries in filter:\r\n", filteredPackets.size());
+                    for (int i=0; i < filteredPackets.size(); i++)
+                        pc.printf("0x%08X ", filteredPackets[i]);
+                    pc.printf("\r\n");
+                    break;
+                }
+                case 'r':
+                case 'R':
+                {
+                    // Restart mbed
+                    pc.printf("Restarting mbed...\r\n");
+                    mbed_reset();
+                    break;
+                }
+                case 't':
+                case 'T':
+                {
+                    // Trigger packet send
+                    notifyUser();
+                    break;
+                }
+                default:
+                    pc.printf("Unknown keypress!\r\n");
+                    break;
+            }
+        }       
+        // Sleep for 20ms repeatedly, as all messages are handled by an interrupt, this prevents
+        // keeping the mbed at full load
+        wait(0.02);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Dec 11 21:31:57 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63cdd78b2dc1
\ No newline at end of file