File content as of revision 0:cbd88c574455:

#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);
    }
}