100 lines
2.8 KiB
Plaintext
100 lines
2.8 KiB
Plaintext
#include <stdio.h>
|
|
|
|
#define SYNC 0xAA
|
|
#define EXCODE 0x55
|
|
|
|
int parsePayload( unsigned char *payload, unsigned char pLength ) {
|
|
|
|
unsigned char bytesParsed = 0;
|
|
unsigned char code;
|
|
unsigned char length;
|
|
unsigned char extendedCodeLevel;
|
|
int i;
|
|
|
|
/* Loop until all bytes are parsed from the payload[] array... */
|
|
while( bytesParsed < pLength ) {
|
|
|
|
/* Parse the extendedCodeLevel, code, and length */
|
|
extendedCodeLevel = 0;
|
|
while( payload[bytesParsed] == EXCODE ) {
|
|
extendedCodeLevel++;
|
|
bytesParsed++;
|
|
}
|
|
code = payload[bytesParsed++];
|
|
if( code & 0x80 ) length = payload[bytesParsed++];
|
|
else length = 1;
|
|
|
|
/* TODO: Based on the extendedCodeLevel, code, length,
|
|
* and the [CODE] Definitions Table, handle the next
|
|
* "length" bytes of data from the payload as
|
|
* appropriate for your application.
|
|
*/
|
|
printf( "EXCODE level: %d CODE: 0x%02X length: %d\n",
|
|
extendedCodeLevel, code, length );
|
|
printf( "Data value(s):" );
|
|
for( i=0; i<length; i++ ) {
|
|
printf( " %02X", payload[bytesParsed+i] & 0xFF );
|
|
}
|
|
printf( "\n" );
|
|
|
|
/* Increment the bytesParsed by the length of the Data Value */
|
|
bytesParsed += length;
|
|
}
|
|
|
|
return( 0 );
|
|
}
|
|
|
|
int main( int argc, char **argv ) {
|
|
|
|
int checksum;
|
|
unsigned char payload[256];
|
|
unsigned char pLength;
|
|
unsigned char c;
|
|
unsigned char i;
|
|
|
|
/* TODO: Initialize 'stream' here to read from a serial data
|
|
* stream, or whatever stream source is appropriate for your
|
|
* application. See documentation for "Serial I/O" for your
|
|
* platform for details.
|
|
*/
|
|
|
|
FILE *stream = 0;
|
|
stream = fopen( "COM4", "r" );
|
|
|
|
/* Loop forever, parsing one Packet per loop... */
|
|
while( 1 ) {
|
|
|
|
/* Synchronize on [SYNC] bytes */
|
|
fread( &c, 1, 1, stream );
|
|
if( c != SYNC ) continue;
|
|
fread( &c, 1, 1, stream );
|
|
if( c != SYNC ) continue;
|
|
|
|
/* Parse [PLENGTH] byte */
|
|
while( true ) {
|
|
fread( &pLength, 1, 1, stream );
|
|
if( pLength ~= 170 ) break;
|
|
}
|
|
if( pLength > 169 ) continue;
|
|
|
|
/* Collect [PAYLOAD...] bytes */
|
|
fread( payload, 1, pLength, stream );
|
|
|
|
/* Compute [PAYLOAD...] chksum */
|
|
checksum = 0;
|
|
for( i=0; i<pLength; i++ ) checksum += payload[i];
|
|
checksum &= 0xFF;
|
|
checksum = ~checksum & 0xFF;
|
|
|
|
/* Parse [CKSUM] byte */
|
|
fread( &c, 1, 1, stream );
|
|
|
|
/* Verify [PAYLOAD...] chksum against [CKSUM] */
|
|
if( c != checksum ) continue;
|
|
|
|
/* Since [CKSUM] is OK, parse the Data Payload */
|
|
parsePayload( payload, pLength );
|
|
}
|
|
|
|
return( 0 );
|
|
} |