Diff: DS2.cpp

Revision:

0:32d3cc3791c4

Child:

1:c935b1619b83

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DS2.cpp	Fri May 20 16:54:39 2011 +0000
@@ -0,0 +1,210 @@
+#include "DS2.h"
+
+
+DS2Packet::DS2Packet(int maxlength)
+{
+    has8BitAddr = 0;
+    rawdata = (char*)malloc(maxlength);
+    address = (unsigned short*)rawdata;
+    length = rawdata + 2;
+    data = rawdata + 3;
+    checksum = rawdata + maxlength - 1;
+    
+    *length = 0;
+}
+    
+DS2Packet::DS2Packet(int address, const char* data, int length)
+{
+    has8BitAddr = 0;
+    rawdata = (char*)malloc(length + 4);
+    this->address = (unsigned short*)rawdata;
+    this->length = rawdata + 2;
+    this->data = rawdata + 3;
+    this->checksum = rawdata + 3 + length;
+    
+    rawdata[0] = address & 0xff;
+    rawdata[1] = (address >> 8) & 0xff;
+    rawdata[2] = length + 4;
+    memcpy(rawdata + 3, data, length);
+    
+    updateChecksum();
+}
+
+DS2Packet::~DS2Packet()
+{
+    free(rawdata);
+}
+
+void DS2Packet::has8BitAddress(bool b)
+{
+    if(b && !has8BitAddr)
+    {
+        *address <<= 8;
+        *length -= 1;
+        rawdata += 1;
+        updateChecksum();
+    }
+    else if(!b && has8BitAddr)
+    {
+        *address >>= 8;
+        *length += 1;
+        rawdata -= 1;
+        updateChecksum();
+    }
+    has8BitAddr = b;
+}
+
+void DS2Packet::updateChecksum()
+{
+    *checksum = 0;
+    for(int i = 0; i < *length - 1; i++)
+        *checksum ^= rawdata[i];
+}
+
+
+DS2::DS2(Bus* KLine, Bus* LLine) :
+    k(KLine), l(LLine)
+{
+    k->baud(9600);
+    l->baud(9600);
+    k->format(8, Serial::Even, 1);
+    l->format(8, Serial::Even, 1);
+}
+
+DS2::~DS2()
+{
+
+}
+
+int DS2::sendPacket(DS2Packet* packet, Bus* bus)
+{
+    dbg.printf("sent: ");
+    for(int i = 0; i < packet->getLength(); i++)
+    {
+        dbg.printf("%02x ", packet->getRawData()[i]);
+    }
+    dbg.printf("on %s bus\r\n", (bus == k)? "k":"l");
+    
+    return bus->send(packet->getRawData(), packet->getLength());
+}
+
+//reads in a whole packet without verification; assumes the whole packet is in the uart buffer
+DS2Packet* DS2::getPacket(Bus* bus)
+{
+    DS2Packet* packet = new DS2Packet(DS2_MTU);
+    int count =  bus->get(packet->getRawData(), DS2_MTU);
+    
+    dbg.printf("received: ");
+    for(int i = 0; i < count; i++)
+        dbg.printf("%02x ", packet->getRawData()[i]);
+    dbg.printf("on %s bus\r\n", (bus == k)? "k":"l");
+    
+    return packet;
+}
+
+DS2Packet* DS2::getPacket8(Bus* bus)
+{
+    DS2Packet* packet = new DS2Packet(DS2_MTU);
+    packet->has8BitAddress(1);
+    int count =  bus->get(packet->getRawData(), DS2_MTU);
+    
+    dbg.printf("received: ");
+    for(int i = 0; i < count; i++)
+        dbg.printf("%02x ", packet->getRawData()[i]);
+    dbg.printf("on %s bus\r\n", (bus == k)? "k":"l");
+    
+    return packet;
+}
+
+//snoop k and l traffic and print it to dbg
+//doesn't return
+void DS2::snoop()
+{
+    dbg.printf("sniffing data on k and l\r\n");
+    dbg.printf("k = <0x??>\r\nl = [0x??]\r\n");
+    dbg.printf("begin INPA traffic now\r\n");
+    int squelch = 0;
+    while(1)
+    {
+        if(k->readable())
+        {
+            dbg.printf("<%02x>", k->getc());
+            squelch = 10000;
+        }
+        else if(l->readable())
+        {
+            dbg.printf("[%02x]", l->getc());
+            squelch = 10000;
+        }
+        else if(squelch)
+        {
+            if(!--squelch)
+                dbg.printf("\r\n");
+        }
+    }
+}
+
+//try to query a module on the two different buses and with the two different packet types
+void DS2::testModule(int address)
+{
+    //assemble an identification request packet
+    DS2Packet *packet = new DS2Packet(address, DS2_IDENTIFY, sizeof(DS2_IDENTIFY));
+    
+    //try on k first
+    dbg.printf("trying module with 16 bit address 0x%02x on k bus\r\n", packet->getAddress());
+    sendPacket(packet, k);
+    wait(0.5);
+    if(k->readable()) //the module sent a reply
+    {
+        DS2Packet* reply = getPacket(k);
+        dbg.printf("received %i bytes from module at 0x%02x on k bus\r\n", reply->getLength(), reply->getAddress());
+        delete reply;
+        
+    }
+
+    //then try with 8 bit address
+    packet->has8BitAddress(1);
+    dbg.printf("trying module with  8 bit address 0x%02x on k bus\r\n", packet->getAddress());
+    sendPacket(packet, k);
+    wait(.05);
+    if(k->readable())
+    {
+        DS2Packet* reply = getPacket8(k);
+        dbg.printf("received %i bytes from module at 0x%02x on k bus\r\n", reply->getLength(), reply->getAddress());
+        delete reply;
+    }
+
+    //now try on l
+    packet->has8BitAddress(0);
+    dbg.printf("trying module with 16 bit address 0x%02x on l bus\r\n", packet->getAddress());
+    sendPacket(packet, l);
+    wait(0.5);
+    if(k->readable())
+    {
+        DS2Packet* reply = getPacket(k);
+        dbg.printf("received %i bytes from module at 0x%02x on k bus\r\n", reply->getLength(), reply->getAddress());
+        delete reply;
+    }
+    
+    //8 bit address again
+    packet->has8BitAddress(1);
+    dbg.printf("trying module with  8 bit address 0x%02x on l bus\r\n", packet->getAddress());
+    sendPacket(packet, l);
+    wait(0.5);
+    if(k->readable())
+    {
+        DS2Packet* reply = getPacket8(k);
+        reply->has8BitAddress(1);
+        dbg.printf("received %i bytes from module at 0x%02x on k bus\r\n", reply->getLength(), reply->getAddress());
+        delete reply;
+    }
+  
+
+    delete packet;
+}
+
+//if I knew of a module that was guaranteed to be the same in every car I'd test querying it here
+bool DS2::test()
+{
+    return 1;
+}
\ No newline at end of file