int main(void)
{
struct prot msg, tmp;
char bindata[256] =
"\x01\x00\x00\x00"
"\x24\x00\x00\x00"
"\x01\x00\x00\x00"
"\x4F\x00\x00\x00"
"some string data in a binary message\x00\x00\x00\x00"
"A\x00\x00\x00\x00\x00\x00\x00"
"more zero-terminated string data in a binary message, the best of both worlds?!\x00\x00";
char shens[256];
packet_decode(&msg, bindata);
packet_encode(shens, &msg, sizeof shens);
packet_decode(&tmp, shens);
dump(bindata, sizeof bindata);
printf(
"\nid=%u, str1len=%u, str2len=%u, str3len=%u\n"
"str1=\"%s\", str2=\"%s\", str3=\"%s\"\n",
msg.head->id, msg.head->str1len, msg.head->str2len,
msg.head->str3len, msg.str1, msg.str2, msg.str3);
dump(shens, sizeof shens);
printf(
"\nid=%u, str1len=%u, str2len=%u, str3len=%u\n"
"str1=\"%s\", str2=\"%s\", str3=\"%s\"\n",
tmp.head->id, tmp.head->str1len, tmp.head->str2len,
tmp.head->str3len, tmp.str1, tmp.str2, tmp.str3);
return 0;
}
int
packet_decode_pcap(Packet *packet, const uint8_t *pkt,
const struct pcap_pkthdr *pkthdr)
{
if (pkthdr == NULL)
return -1;
return packet_decode(packet, pkt, pkthdr->caplen);
}
END_TEST
START_TEST(test_packet_decode_with_zero_size)
{
Packet *packet = packet_create( );
int err = packet_decode(packet, pkt10, 0);
fail_unless((err != 0), "packet_decode returned OK expected ERROR");
packet_destroy(packet);
}
END_TEST
START_TEST(test_packet_decode_with_null_data_pointer)
{
Packet *packet = packet_create( );
int err = packet_decode(packet, NULL, sizeof pkt10);
fail_unless((err != 0), "packet_decode returned OK expected ERROR");
packet_destroy(packet);
}
END_TEST
/* XXX
* This test isn't really supposed to be here but its the best way to
* validate that the decode went through the entire packet */
START_TEST(test_packet_proto_count)
{
Packet *packet = packet_create( );
int err = packet_decode(packet, pkt10, sizeof pkt10);
fail_unless((err == 0), "packet_decode returned ERROR");
err = packet_proto_count(packet);
fail_unless((err == 5),
"packet_proto_count(packet) returned %d, expected 5", err);
unsigned i;
Protocol *proto = packet_proto_first(packet, &i);
fail_unless((packet_proto_proto(proto) == PROTO_ETH),
"layer %d not ETH");
proto = packet_proto_next(packet, &i);
fail_unless((packet_proto_proto(proto) == PROTO_PPPOE),
"layer %d not PPPOE");
proto = packet_proto_next(packet, &i);
fail_unless((packet_proto_proto(proto) == PROTO_PPP),
"layer %d not PPP");
proto = packet_proto_next(packet, &i);
fail_unless((packet_proto_proto(proto) == PROTO_IP4),
"layer %d not IP4");
proto = packet_proto_next(packet, &i);
fail_unless((packet_proto_proto(proto) == PROTO_TCP),
"layer %d not TCP");
packet_destroy(packet);
}
int main(int argc,char *argv[]) {
int argno;
fprintf(stderr, "SPI decoder, V%s\n", VERSION);
argno = HandleOptions(argc,argv);
if (fileread) {
if ((datfile = fopen(DATFILENAME,"r")) == NULL) // opne to read from .dat file
fatal_err(DATFILENAME " open for read failed");
fprintf(stderr, "Reading from " DATFILENAME "\n");
}
else {
char dev_name[80];
sprintf(dev_name, "\\\\.\\COM%d", comport);
fprintf(stderr, "Opening serial port on %s...", dev_name);
handle_serial = CreateFile(dev_name, GENERIC_READ | GENERIC_WRITE, 0, 0,
OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
if (handle_serial!=INVALID_HANDLE_VALUE) {
dcbSerialParams.BaudRate = 115200;
dcbSerialParams.ByteSize = 8;
dcbSerialParams.StopBits = ONESTOPBIT;
dcbSerialParams.Parity = NOPARITY;
dcbSerialParams.DCBlength = sizeof(DCB);
if(SetCommState(handle_serial, &dcbSerialParams) == 0) fatal_err("Error setting serial port parameters");
timeouts.ReadIntervalTimeout = 100; // msec
timeouts.ReadTotalTimeoutConstant = 200; // msec
timeouts.ReadTotalTimeoutMultiplier = 0; // msec
timeouts.WriteTotalTimeoutConstant = 50;
timeouts.WriteTotalTimeoutMultiplier = 10;
if(SetCommTimeouts(handle_serial, &timeouts) == 0) fatal_err("Error setting serial port timeouts");
fprintf(stderr,"OK\n");
}
else fatal_err("Failed");
if ((datfile = fopen(DATFILENAME,"a")) == NULL) // open to append to .dat file
fatal_err(DATFILENAME " open for append failed");
}
if ((outfile = fopen(OUTFILENAME,"a")) == NULL) fatal_err(OUTFILENAME " open failed");
if ((pktfile = fopen(PKTFILENAME,"a")) == NULL) fatal_err(PKTFILENAME " open failed");
fprintf(pktfile, "\n");
// atexit(cleanup);
fprintf(stderr, "Starting.\n");
while(!kbhit()) {
more_data:
if (fileread) { // read from .dat file
if (!fgets(line, MAX_LINE, datfile)) {
output("***end of file");
fprintf(stderr, "***end of file");
cleanup();
exit(0);
}
++linecnt;
bytes_read = strlen(line);
output("got %d bytes from the file\n", bytes_read);
}
else { // read from serial port
// printf("reading serial port com%d...\n", comport);
ReadFile(handle_serial, line, MAX_LINE, &bytes_read, NULL);
line[bytes_read]='\0';
if (bytes_read != 0) {
output("got %d bytes from serial port\n", bytes_read);
fprintf(stderr, "got %d bytes from the serial port\n", bytes_read);
fprintf(datfile, "%s\n", line);
}
}
if (strcmp(line, "SPI Sniffer\n") == 0) {
fprintf(stderr, "\"SPI Sniffer\" header line read\n");
continue;
}
// output("decode %n bytes: %s\n", bytes_read, line);
lineptr = line;
while (*lineptr != '\0') {
skip_to_next_data(); // process input up to next master/slave data pair
if (*lineptr == '\0')break;
next_command:
if (!read_data_pair()) goto more_data;
isread = master_data & 0x80; // "read register" flag bit
isburst = master_data & 0x40; // "burst" flag bit
regnum = master_data & 0x3f; // register number 0 to 63
if (isread) { // config register read
if (regnum >= 0x30 && regnum <= 0x3d && !isburst) { // no, is really command strobe
command_strobe();
}
else {
if(isburst){
if (regnum == 0x3f) { // read RX FIFO: receive packet
if (!chip_selected) exit_msg("burst RX FIFO write without chip selected", regnum);
show_config_reg("read", true);
packet.xmit = false;
while (1) { // show all burst read data from FIFO
if (!skip_timestamp()) goto next_command;
if (*lineptr == ']') break; // ends with chip unselect
if (!read_data_pair()) goto next_command;
output(" %02X", slave_data);
if (packet.length < MAX_PKT) {
packet.data[packet.length++] = slave_data;
}
}
output("\n");
packet_decode();
}
else { // burst read of other than FIFO: consecutive config registers
if (!skip_to_next_data()) goto next_command;
while (1) {
if (!skip_timestamp()) goto next_command;
if (*lineptr == ']') break; // ends with chip unselect
if (!read_data_pair()) goto next_command;
regval = slave_data;
show_config_reg("read", false);
if (++regnum >= 0x40) exit_msg("burst read of too many config registers", regnum);
}
}
}
else { // regular single-register read
if (!read_data_pair()) goto next_command;
regval = slave_data;
show_config_reg("read", false);
}
}
}
else { // register write
if (regnum >= 0x30 && regnum <= 0x3d && !isburst) { // no, is really command strobe
command_strobe();
}
else if (regnum == 0x3e) { // write power table
if (isburst) {
if (!chip_selected) exit_msg("burst power table write without chip selected", regnum);
show_config_reg("write", true);
while (1) { // show all burst write data to power table
if (!skip_timestamp()) goto next_command;
if (*lineptr == ']') break; // ends with chip unselect
if (!read_data_pair()) goto next_command;
output(" %02X", master_data);
}
output("\n");
}
else { // non-burst write to power table
if (!read_data_pair()) goto next_command;
regval = master_data;
show_config_reg("write", false);
}
}
else if (regnum == 0x3f) { // write TX FIFO: transmit packet
if (!isburst) exit_msg("implement non-burst TX FIFO write", regnum);
if (!chip_selected) exit_msg("burst TX FIFO write without chip selected", regnum);
show_config_reg("write", true);
packet.xmit = true;
while (1) { // show all burst write data to FIFO
if (!skip_timestamp()) goto next_command;
if (*lineptr == ']') break; // ends with chip unselect
if (!read_data_pair()) goto next_command;
output(" %02X", master_data);
if (packet.length < MAX_PKT) {
packet.data[packet.length++] = master_data;
}
}
output("\n");
packet_decode();
}
else { // writing config register(s)
if (isburst) { // burst config register write
int bytes_bursted, bytes_changed, start_reg, end_reg;
bytes_bursted = 0;
bytes_changed = 0;
start_reg = regnum;
// output("burst config write\n");
if (!chip_selected) output("burst write without chip selected at reg %02X", regnum);
while (1) { // read all the burst write data
if (!skip_timestamp()) goto next_command;
if (*lineptr == ']') break; // ends with chip unselect
if (regnum > 0x2e) exit_msg("too much burst data", regnum);
if (!read_data_pair()) goto next_command;
new_config_regs[regnum++] = master_data;
++bytes_bursted;
}
end_reg = regnum-1;
for (regnum=start_reg; regnum<end_reg; ++regnum) { // show only those that changed
if ((new_config_regs[regnum] != current_config_regs[regnum])) {
regval = new_config_regs[regnum];
show_config_reg(" wrote", false);
current_config_regs[regnum] = new_config_regs[regnum];
++bytes_changed;
}
}
show_delta_time();
output(" burst wrote %d registers, and %d changed\n", bytes_bursted, bytes_changed);
}
else { // single register write
if (!read_data_pair()) goto next_command;
regval = master_data;
show_config_reg("write", false);
current_config_regs[regnum] = regval;
}
}
}
}
}
return 0;
}
void setup( )
{
packet = packet_create( );
packet_decode(packet, pkt10, sizeof pkt10);
}
/*!
\brief This functions handles all incoing data from the ECU and validates
its content for proper START/STOP/ESCAPING and allocates a FreeEMS_Packet
structure for VALID packets and populates the required fields as needed
\param buf is a pointer to the incoming data buffer
\param len is the numbe of bytes to pull from the incoming buffer
*/
G_MODULE_EXPORT void handle_data(guchar *buf, gint len)
{
static GAsyncQueue *queue = NULL;
/* Statistic collection variables */
static guchar packetBuffer[3000];
static unsigned int packets = 0;
static unsigned int charsDropped = 0;
static unsigned int badChecksums = 0;
static unsigned int badPackets = 0;
static unsigned int goodChecksums = 0;
static unsigned int startsInsidePacket = 0;
static unsigned int totalFalseStartLost = 0;
static unsigned int doubleStartByteOccurances = 0;
static unsigned int strayDataBytesOccurances = 0;
static unsigned int escapeBytesFound = 0;
static unsigned int escapedStopBytesFound = 0;
static unsigned int escapedStartBytesFound = 0;
static unsigned int escapedEscapeBytesFound = 0;
static unsigned int escapePairMismatches = 0;
static unsigned long sumOfGoodPacketLengths = 0;
/* Loop and state variables */
static gboolean insidePacket = FALSE;
static gboolean unescapeNext = FALSE;
static unsigned int processed = 0;
static unsigned char checksum = 0;
static unsigned char lastChar = 0;
static unsigned int currentPacketLength = 0;
guchar character;
gint i = 0;
FreeEMS_Packet *packet = NULL;
if (!queue)
queue = (GAsyncQueue *)DATA_GET(global_data,"packet_queue");
log_inbound_data_f(buf,len);
for (i=0;i<len;i++)
{
character = buf[i];
if (character == START_BYTE)
{
if (insidePacket)
{
startsInsidePacket++;
if (currentPacketLength == 0)
{
doubleStartByteOccurances++;
}
else
{
totalFalseStartLost += currentPacketLength;
strayDataBytesOccurances++;
}
}
insidePacket = TRUE;
checksum = 0;
currentPacketLength = 0;
}
else if (insidePacket)
{
if (unescapeNext)
{ /* Clear escaped byte next flag */
unescapeNext = FALSE;
if (character == ESCAPED_ESCAPE_BYTE)
{
checksum += ESCAPE_BYTE;
lastChar = ESCAPE_BYTE;
escapedEscapeBytesFound++;
packetBuffer[currentPacketLength] = ESCAPE_BYTE;
currentPacketLength++;
}
else if (character == ESCAPED_START_BYTE)
{
/* Store and checksum start byte */
checksum += START_BYTE;
lastChar = START_BYTE;
escapedStartBytesFound++;
packetBuffer[currentPacketLength] = START_BYTE;
currentPacketLength++;
}
else if(character == ESCAPED_STOP_BYTE)
{
/* Store and checksum stop byte */
checksum += STOP_BYTE;
lastChar = STOP_BYTE;
escapedStopBytesFound++;
packetBuffer[currentPacketLength] = STOP_BYTE;
currentPacketLength++;
}else
{
/* Otherwise reset and record as data is bad */
insidePacket = FALSE;
checksum = 0;
currentPacketLength = 0;
escapePairMismatches++;
}
}
else if (character == ESCAPE_BYTE)
{
/* Set flag to indicate that the next byte should be un-escaped. */
unescapeNext = TRUE;
escapeBytesFound++;
}
else if (character == STOP_BYTE)
{
packets++;
/* Bring the checksum back to where it should be */
checksum -= lastChar;
/* Check that the checksum matches */
if(checksum != lastChar)
{
badChecksums++;
printf("Packet number %u ending of length %u at char number %u failed checksum! Received %u Calculated %u\n", packets, currentPacketLength, processed, lastChar, checksum);
}
else
{
goodChecksums++;
/* Add the length to the SUM */
sumOfGoodPacketLengths += currentPacketLength;
/* Clear the state */
packet = g_new0(FreeEMS_Packet, 1);
packet->data = (guchar *)g_memdup(packetBuffer,currentPacketLength);
packet->raw_length = currentPacketLength;
mtxlog_packet(packet->data,packet->raw_length,FALSE);
if (!packet_decode(packet))
{
printf("Packet fields don't make sense!\n");
freeems_packet_cleanup(packet);
badPackets++;
}
else if (queue)
{
g_async_queue_ref(queue);
g_async_queue_push(queue,(gpointer)packet);
g_async_queue_unref(queue);
}
else
printf("packet queue not found!?!!\n");
}
insidePacket = FALSE;
currentPacketLength= 0;
checksum = 0;
}
else
{
/* If it isn't special checksum it! */
checksum += character;
lastChar = character;
packetBuffer[currentPacketLength] = character;
currentPacketLength++;
}
}
else
charsDropped++;
}
}
