overhauled CFontz633io.c: doxygen'ated it, added more check for cyclical fifos, ...
This commit is contained in:
marschap
+227
-244
@@ -26,15 +26,16 @@
|
||||
|
||||
COMMAND_PACKET incoming_command;
|
||||
|
||||
/* variables for circular receive buffer */
|
||||
#define RECEIVEBUFFERSIZE 512
|
||||
unsigned char SerialReceiveBuffer[RECEIVEBUFFERSIZE];
|
||||
int ReceiveBufferHead;
|
||||
int ReceiveBufferTail;
|
||||
int ReceiveBufferTailPeek;
|
||||
int ReceiveBufferHead = 0;
|
||||
int ReceiveBufferTail = 0;
|
||||
int ReceiveBufferTailPeek = 0;
|
||||
|
||||
|
||||
/*
|
||||
* KeyRing handeling function.
|
||||
* KeyRing handling function.
|
||||
* This separate the producer from the consumer.
|
||||
* It is just a small fifo of unsigned char.
|
||||
*/
|
||||
@@ -44,13 +45,15 @@ unsigned char KeyRing[KEYRINGSIZE];
|
||||
int KeyHead = 0;
|
||||
int KeyTail = 0;
|
||||
|
||||
/*-OK-----------------------------------------------------------------------*/
|
||||
|
||||
/** initialize/empty key ring by resetting its read & write pointers */
|
||||
void EmptyKeyRing(void)
|
||||
{
|
||||
KeyHead = KeyTail = 0;
|
||||
}
|
||||
|
||||
/*-OK-----------------------------------------------------------------------*/
|
||||
|
||||
/** add byte to key ring; return success (byte added) / failure (key ring is full) */
|
||||
int AddKeyToKeyRing(unsigned char key)
|
||||
{
|
||||
if (((KeyHead + 1) % KEYRINGSIZE) != (KeyTail % KEYRINGSIZE)) {
|
||||
@@ -64,13 +67,16 @@ int AddKeyToKeyRing(unsigned char key)
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*-OK-----------------------------------------------------------------------*/
|
||||
|
||||
/** get byte from key ring (or '\0' if key ring is empty) */
|
||||
unsigned char GetKeyFromKeyRing(void)
|
||||
{
|
||||
int retval = 0;
|
||||
unsigned char retval = '\0';
|
||||
|
||||
if ((KeyHead % KEYRINGSIZE) != (KeyTail % KEYRINGSIZE )) {
|
||||
retval = KeyRing[KeyTail % KEYRINGSIZE];
|
||||
KeyTail %= KEYRINGSIZE;
|
||||
|
||||
if ((KeyHead % KEYRINGSIZE) != KeyTail) {
|
||||
retval = KeyRing[KeyTail];
|
||||
KeyTail = (KeyTail + 1) % KEYRINGSIZE;
|
||||
}
|
||||
|
||||
@@ -81,46 +87,49 @@ unsigned char GetKeyFromKeyRing(void)
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* ----------------------------------------------------------------------------
|
||||
* send_packet() send_packet() will send set the CRC in outgoing_response
|
||||
* and send it to the host.
|
||||
* ----------------------------------------------------------------------------
|
||||
*/
|
||||
/** send message with arguments to the given handle */
|
||||
void send_bytes_message(int fd, int len, int msg, char *framebuf)
|
||||
{
|
||||
int i;
|
||||
outgoing_response.command=msg;
|
||||
outgoing_response.data_length=len;
|
||||
for (i = 0; i < outgoing_response.data_length; i++) {
|
||||
outgoing_response.data[i]=framebuf[i];
|
||||
}
|
||||
|
||||
outgoing_response.command = msg;
|
||||
outgoing_response.data_length = len;
|
||||
for (i = 0; i < outgoing_response.data_length; i++)
|
||||
outgoing_response.data[i] = framebuf[i];
|
||||
|
||||
/* send message & calc CRC */
|
||||
send_packet(fd);
|
||||
}
|
||||
|
||||
|
||||
/** send message with one byte argument to the given handle */
|
||||
void send_onebyte_message(int fd, int msg, int value)
|
||||
{
|
||||
outgoing_response.command=msg;
|
||||
outgoing_response.data_length=1;
|
||||
outgoing_response.data[0]=value;
|
||||
outgoing_response.command = msg;
|
||||
outgoing_response.data_length = 1;
|
||||
outgoing_response.data[0] = value;
|
||||
|
||||
/* send message & calc CRC */
|
||||
send_packet(fd);
|
||||
}
|
||||
|
||||
|
||||
/** send message without data to the given handle */
|
||||
void send_zerobyte_message(int fd, int msg)
|
||||
{
|
||||
outgoing_response.command=msg;
|
||||
outgoing_response.data_length=0;
|
||||
outgoing_response.command = msg;
|
||||
outgoing_response.data_length = 0;
|
||||
|
||||
/* send message & calc CRC */
|
||||
send_packet(fd);
|
||||
}
|
||||
|
||||
|
||||
/*----------------------------------------------------------------------------*/
|
||||
int get_crc(char *bufptr, int len, int seed)
|
||||
/** calculate CRC over given buffer with given length */
|
||||
int get_crc(char *buf, int len, int seed)
|
||||
{
|
||||
|
||||
/*
|
||||
* CRC lookup table to avoid bit-shifting loops.
|
||||
*/
|
||||
/* CRC lookup table to avoid bit-shifting loops. */
|
||||
static const word crcLookupTable[256] = {
|
||||
0x00000, 0x01189, 0x02312, 0x0329B, 0x04624, 0x057AD, 0x06536, 0x074BF,
|
||||
0x08C48, 0x09DC1, 0x0AF5A, 0x0BED3, 0x0CA6C, 0x0DBE5, 0x0E97E, 0x0F8F7,
|
||||
@@ -156,54 +165,31 @@ int get_crc(char *bufptr, int len, int seed)
|
||||
0x07BC7, 0x06A4E, 0x058D5, 0x0495C, 0x03DE3, 0x02C6A, 0x01EF1, 0x00F78
|
||||
};
|
||||
|
||||
/*
|
||||
* Initial value of the CRC.
|
||||
*/
|
||||
int newCrc;
|
||||
/* initialize the new crc with the given seed */
|
||||
int newCrc = seed;
|
||||
|
||||
/*
|
||||
* seed should be 0xFFFF;
|
||||
*/
|
||||
newCrc = seed;
|
||||
/* This algorithm is based on the IrDA LAP example */
|
||||
while (len-- > 0)
|
||||
newCrc = (newCrc >> 8) ^ crcLookupTable[(newCrc ^ *buf++) & 0xff];
|
||||
|
||||
/*
|
||||
* This algorithim is based on the IrDA LAP example.
|
||||
*/
|
||||
while (len--)
|
||||
newCrc =
|
||||
(newCrc >> 8) ^ crcLookupTable[(newCrc ^ *bufptr++) & 0xff];
|
||||
|
||||
/*
|
||||
* Make this crc match the one's complement that is sent in the packet.
|
||||
*/
|
||||
/* Make this crc match the one's complement that is sent in the packet */
|
||||
return (~newCrc);
|
||||
}
|
||||
|
||||
|
||||
/** send one byte to the given handle */
|
||||
void
|
||||
SendByte(int fd, unsigned char datum)
|
||||
{
|
||||
int bytes_written;
|
||||
bytes_written = 0;
|
||||
bytes_written = write(fd, &datum, 1);
|
||||
/* if (bytes_written != 1) {
|
||||
printf("SendByte(): only %d of %d bytes sent.", bytes_written, 1);
|
||||
} */
|
||||
|
||||
return;
|
||||
write(fd, &datum, 1);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
|
||||
COMMAND_PACKET outgoing_response;
|
||||
|
||||
/*
|
||||
* ============================================================================
|
||||
* send_packet() send_packet() will send set the CRC in outgoing_response
|
||||
* and send it to the host.
|
||||
* ----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
/** send outgoing_response to the given handle; calc & send CRC when doing so */
|
||||
void
|
||||
send_packet(int fd)
|
||||
{
|
||||
@@ -214,19 +200,17 @@ send_packet(int fd)
|
||||
for (i = 0; i < outgoing_response.data_length; i++) {
|
||||
SendByte(fd, outgoing_response.data[i]);
|
||||
}
|
||||
/*
|
||||
* Set the CRC
|
||||
*/
|
||||
outgoing_response.CRC.as_word =
|
||||
get_crc((unsigned char *) & outgoing_response,
|
||||
|
||||
/* calculate & send the CRC */
|
||||
outgoing_response.CRC.as_word = get_crc((unsigned char *) &outgoing_response,
|
||||
outgoing_response.data_length + 2, 0xFFFF);
|
||||
SendByte(fd, outgoing_response.CRC.as_bytes[0]);
|
||||
SendByte(fd, outgoing_response.CRC.as_bytes[1]);
|
||||
|
||||
/**** TEST STUF ****/
|
||||
// print_packet(&outgoing_response);
|
||||
/**** TEST STUF ****/
|
||||
// print_packet(&outgoing_response);
|
||||
|
||||
/* Every time we send a message, we also check for incomming one. */
|
||||
/* Every time we send a message, we also check for an incoming one. */
|
||||
test_packet(fd);
|
||||
}
|
||||
|
||||
@@ -254,155 +238,152 @@ send_packet(int fd)
|
||||
/* -------------------------------------------------- */
|
||||
/* ^ Tail ^ Head */
|
||||
|
||||
/*-OK-----------------------------------------------------------------------*/
|
||||
void EmptyReceiveBuffer(void)
|
||||
{
|
||||
ReceiveBufferHead=ReceiveBufferTail=0;
|
||||
}
|
||||
|
||||
/*-OK------------------------------------------------------------------------
|
||||
* Gets incoming data from Window's ReadFile() and puts it into
|
||||
* SerialReceiveBuffer[].
|
||||
*/
|
||||
/** initialize/empty receive buffer by resetting its pointers */
|
||||
void EmptyReceiveBuffer(void)
|
||||
{
|
||||
ReceiveBufferHead = ReceiveBufferTail = ReceiveBufferTailPeek = 0;
|
||||
}
|
||||
|
||||
|
||||
/** read given number of bytes from given file handle into receive buffer */
|
||||
void Sync_Read_Buffer(int fd, unsigned char expected_bytes)
|
||||
{
|
||||
{
|
||||
unsigned char Incoming[512];
|
||||
int BytesRead;
|
||||
|
||||
// ToDo: check that expected_bytes < sizeof(Incoming)
|
||||
BytesRead = read(fd, Incoming, expected_bytes);
|
||||
if (BytesRead == -1){
|
||||
/* printf("~~~Problem reading: %s .\n", strerror(errno)); */
|
||||
}
|
||||
else {
|
||||
int i;
|
||||
|
||||
BytesRead = read(fd, Incoming, expected_bytes);
|
||||
if (BytesRead==-1){
|
||||
/* printf("~~~Problem reading: %s .\n", strerror(errno)); */
|
||||
}
|
||||
else
|
||||
{
|
||||
/* printf("Readed %d Bytes:", BytesRead); */
|
||||
/* printf("Read %d Bytes:", BytesRead); */
|
||||
|
||||
/* Read the incoming unsigned char, store it, */
|
||||
for(i=0; i<BytesRead; i++)
|
||||
{
|
||||
/* printf(" %02x", Incoming[i]); */
|
||||
SerialReceiveBuffer[ReceiveBufferHead]=Incoming[i];
|
||||
/* Increment the pointer and wrap if needed. */
|
||||
ReceiveBufferHead++;
|
||||
if(RECEIVEBUFFERSIZE<=ReceiveBufferHead)
|
||||
ReceiveBufferHead=0;
|
||||
}
|
||||
/* printf("\n"); */
|
||||
/* wrap write pointer to the receive buffer */
|
||||
ReceiveBufferHead %= RECEIVEBUFFERSIZE;
|
||||
|
||||
/* Read the incoming byte and store it, */
|
||||
for (i = 0; i < BytesRead; i++) {
|
||||
/* printf(" %02x", Incoming[i]); */
|
||||
SerialReceiveBuffer[ReceiveBufferHead] = Incoming[i];
|
||||
|
||||
/* increment write pointer (wrap if needed) */
|
||||
ReceiveBufferHead = (ReceiveBufferHead + 1) % RECEIVEBUFFERSIZE;
|
||||
}
|
||||
/* printf("\n"); */
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*-OK-----------------------------------------------------------------------*/
|
||||
/** return number of bytes available for reading in receive buffer */
|
||||
int BytesAvail(void)
|
||||
{
|
||||
int return_value;
|
||||
{
|
||||
int avail_bytes = ReceiveBufferHead - ReceiveBufferTail;
|
||||
|
||||
return_value=ReceiveBufferHead-ReceiveBufferTail;
|
||||
if(return_value<0)
|
||||
return_value+=RECEIVEBUFFERSIZE;
|
||||
return(return_value);
|
||||
}
|
||||
if (avail_bytes < 0)
|
||||
avail_bytes += RECEIVEBUFFERSIZE;
|
||||
|
||||
/*-OK-----------------------------------------------------------------------*/
|
||||
return(avail_bytes % RECEIVEBUFFERSIZE);
|
||||
}
|
||||
|
||||
|
||||
/** get next byte from receive buffer (return '\0' if buffer is empty) */
|
||||
unsigned char GetByte(void)
|
||||
{
|
||||
unsigned char return_byte;
|
||||
{
|
||||
unsigned char return_byte = '\0';
|
||||
|
||||
return_byte=0;
|
||||
/* wrap read pointer to the receive buffer */
|
||||
ReceiveBufferTail %= RECEIVEBUFFERSIZE;
|
||||
|
||||
/* See if there are any more bytes available. */
|
||||
if(ReceiveBufferTail!=ReceiveBufferHead)
|
||||
{
|
||||
/* There is at least one more ubyte. */
|
||||
return_byte=SerialReceiveBuffer[ReceiveBufferTail];
|
||||
|
||||
/* Increment the pointer and wrap if needed. */
|
||||
ReceiveBufferTail++;
|
||||
if(RECEIVEBUFFERSIZE<=ReceiveBufferTail)
|
||||
ReceiveBufferTail=0;
|
||||
}
|
||||
|
||||
return(return_byte);
|
||||
}
|
||||
|
||||
/*-OK-----------------------------------------------------------------------*/
|
||||
unsigned char DiscardGetByte(void)
|
||||
{
|
||||
unsigned char return_byte;
|
||||
|
||||
return_byte=0;
|
||||
|
||||
/* See if there are any more bytes available. */
|
||||
if(ReceiveBufferTail!=ReceiveBufferHead)
|
||||
{
|
||||
/* There is at least one more ubyte. */
|
||||
return_byte=SerialReceiveBuffer[ReceiveBufferTail];
|
||||
|
||||
/* Increment the pointer and wrap if needed. */
|
||||
ReceiveBufferTail++;
|
||||
if(RECEIVEBUFFERSIZE<=ReceiveBufferTail)
|
||||
ReceiveBufferTail=0;
|
||||
}
|
||||
|
||||
/* printf("Discarded : /%02x/\n", return_byte); */
|
||||
|
||||
return(return_byte);
|
||||
}
|
||||
|
||||
|
||||
/*-OK--------------------------------------------------------------------------*/
|
||||
int PeekBytesAvail(void)
|
||||
{
|
||||
int return_value;
|
||||
|
||||
return_value=ReceiveBufferHead-ReceiveBufferTailPeek;
|
||||
|
||||
if(return_value<0)
|
||||
return_value+=RECEIVEBUFFERSIZE;
|
||||
|
||||
return(return_value);
|
||||
}
|
||||
|
||||
/*-OK--------------------------------------------------------------------------*/
|
||||
void Sync_Peek_Pointer(void)
|
||||
{
|
||||
ReceiveBufferTailPeek=ReceiveBufferTail;
|
||||
}
|
||||
|
||||
/*-OK--------------------------------------------------------------------------*/
|
||||
void AcceptPeekedData(void)
|
||||
{
|
||||
ReceiveBufferTail=ReceiveBufferTailPeek;
|
||||
}
|
||||
|
||||
/*-OK--------------------------------------------------------------------------*/
|
||||
unsigned char PeekByte(void)
|
||||
{
|
||||
unsigned char return_byte;
|
||||
|
||||
return_byte=0;
|
||||
|
||||
/* See if there are any more bytes available. */
|
||||
if(ReceiveBufferTailPeek!=ReceiveBufferHead)
|
||||
{
|
||||
if (ReceiveBufferTail != (ReceiveBufferHead % RECEIVEBUFFERSIZE)) {
|
||||
/* There is at least one more byte. */
|
||||
return_byte=SerialReceiveBuffer[ReceiveBufferTailPeek];
|
||||
return_byte = SerialReceiveBuffer[ReceiveBufferTail];
|
||||
|
||||
/* Increment the pointer and wrap if needed. */
|
||||
ReceiveBufferTailPeek++;
|
||||
if(RECEIVEBUFFERSIZE<=ReceiveBufferTailPeek)
|
||||
ReceiveBufferTailPeek=0;
|
||||
/* Increment read pointer (wrap if needed) */
|
||||
ReceiveBufferTail = (ReceiveBufferTail + 1) % RECEIVEBUFFERSIZE;
|
||||
}
|
||||
|
||||
return(return_byte);
|
||||
}
|
||||
|
||||
|
||||
/** get byte from receive buffer (or '\0' if buffer is empty) */
|
||||
unsigned char DiscardGetByte(void)
|
||||
{
|
||||
unsigned char return_byte = '\0';
|
||||
|
||||
|
||||
/* wrap read pointer to the receive buffer */
|
||||
ReceiveBufferTail %= RECEIVEBUFFERSIZE;
|
||||
|
||||
/* See if there are any more bytes available. */
|
||||
if (ReceiveBufferTail != (ReceiveBufferHead % RECEIVEBUFFERSIZE)) {
|
||||
/* There is at least one more ubyte. */
|
||||
return_byte = SerialReceiveBuffer[ReceiveBufferTail];
|
||||
|
||||
/* Increment read pointer (wrap if needed) */
|
||||
ReceiveBufferTail = (ReceiveBufferTail + 1) % RECEIVEBUFFERSIZE;
|
||||
}
|
||||
/* printf("Discarded : /%02x/\n", return_byte); */
|
||||
|
||||
return(return_byte);
|
||||
}
|
||||
|
||||
|
||||
/** return number of bytes available for peeking in receive buffer */
|
||||
int PeekBytesAvail(void)
|
||||
{
|
||||
int avail_bytes = ReceiveBufferHead - ReceiveBufferTailPeek;
|
||||
|
||||
if (avail_bytes < 0)
|
||||
avail_bytes += RECEIVEBUFFERSIZE;
|
||||
|
||||
return(avail_bytes % RECEIVEBUFFERSIZE);
|
||||
}
|
||||
|
||||
|
||||
/** sync peek pointer with read pointer */
|
||||
void Sync_Peek_Pointer(void)
|
||||
{
|
||||
ReceiveBufferTailPeek = ReceiveBufferTail;
|
||||
}
|
||||
|
||||
|
||||
/** accept ppeked data by syncing the read pointer to the peek pointer */
|
||||
void AcceptPeekedData(void)
|
||||
{
|
||||
ReceiveBufferTail = ReceiveBufferTailPeek;
|
||||
}
|
||||
|
||||
|
||||
/** peek next byte from receive buffer (return '\0' if buffer is empty) */
|
||||
unsigned char PeekByte(void)
|
||||
{
|
||||
unsigned char return_byte = '\0';
|
||||
|
||||
/* wrap peek pointer to the receive buffer */
|
||||
ReceiveBufferTailPeek %= RECEIVEBUFFERSIZE;
|
||||
|
||||
/* See if there are any more bytes available. */
|
||||
if (ReceiveBufferTailPeek != (ReceiveBufferHead % RECEIVEBUFFERSIZE)) {
|
||||
/* There is at least one more byte. */
|
||||
return_byte = SerialReceiveBuffer[ReceiveBufferTailPeek];
|
||||
|
||||
/* Increment the peek pointer (wrap if needed). */
|
||||
ReceiveBufferTailPeek = (ReceiveBufferTailPeek + 1) % RECEIVEBUFFERSIZE;
|
||||
}
|
||||
|
||||
return(return_byte);
|
||||
}
|
||||
|
||||
|
||||
void treat_packet(void)
|
||||
{
|
||||
if(incoming_command.command==0x80) {
|
||||
if (incoming_command.command == 0x80) {
|
||||
AddKeyToKeyRing(incoming_command.data[0]);
|
||||
}
|
||||
}
|
||||
@@ -419,7 +400,7 @@ if(incoming_command.command==0x80) {
|
||||
*/
|
||||
|
||||
/* This tossed byte is not in use, it's going to be removed. */
|
||||
int tossed=0;
|
||||
int tossed = 0;
|
||||
|
||||
/* Let's return
|
||||
* O if we have no message but we should try again immediatly
|
||||
@@ -434,7 +415,7 @@ int tossed=0;
|
||||
#define GIVE_UP 2
|
||||
|
||||
unsigned char check_for_packet(int fd, unsigned char expected_length)
|
||||
{
|
||||
{
|
||||
int i;
|
||||
int testcrc;
|
||||
|
||||
@@ -442,64 +423,67 @@ unsigned char check_for_packet(int fd, unsigned char expected_length)
|
||||
|
||||
//First off, there must be at least 4 bytes available in the input stream
|
||||
//for there to be a valid command in it (command, length, no data, CRC).
|
||||
if(BytesAvail()<4) {
|
||||
/* printf("Not enough byte available for even the smallest message.\n"); */
|
||||
if (BytesAvail() < 4) {
|
||||
/* printf("Not enough byte available for even the smallest message.\n"); */
|
||||
return(GIVE_UP); /* We don't need to retry before more byte are received */
|
||||
}
|
||||
|
||||
/* Look into the buffer without removing the data. */
|
||||
Sync_Peek_Pointer();
|
||||
|
||||
//Only commands 0 through MAX_COMMAND are valid.
|
||||
if(MAX_COMMAND<(0x3F&(incoming_command.command=PeekByte())))
|
||||
{
|
||||
/* look at potential command byte */
|
||||
incoming_command.command = PeekByte();
|
||||
|
||||
/* Only commands 0 through MAX_COMMAND are valid */
|
||||
if (MAX_COMMAND < (0x3F & incoming_command.command)) {
|
||||
/* Throw out one byte of garbage. Next pass through should re-sync. */
|
||||
DiscardGetByte();
|
||||
tossed++;
|
||||
/* printf("###: Unknown command.\n"); */
|
||||
return(TRY_AGAIN);
|
||||
}
|
||||
/* There is a valid command byte. Get the data_length. */
|
||||
/* The data length must be within reason. */
|
||||
if(MAX_DATA_LENGTH<(incoming_command.data_length=PeekByte()))
|
||||
{
|
||||
//Throw out one byte of garbage. Next pass through should re-sync.
|
||||
DiscardGetByte();
|
||||
tossed++;
|
||||
/* printf("###: Too long packet: %d.\n", incoming_command.data_length); */
|
||||
/* printf("###: Unknown command.\n"); */
|
||||
return(TRY_AGAIN);
|
||||
}
|
||||
|
||||
//Now there must be at least incoming_command.data_length+sizeof(CRC) bytes
|
||||
//still available for us to continue.
|
||||
if((int)PeekBytesAvail()<(incoming_command.data_length+2)) {
|
||||
/* There is a valid command byte. Get the data_length. */
|
||||
incoming_command.data_length = PeekByte();
|
||||
|
||||
/* The data length must be within reason. */
|
||||
if (MAX_DATA_LENGTH < incoming_command.data_length) {
|
||||
//Throw out one byte of garbage. Next pass through should re-sync.
|
||||
DiscardGetByte();
|
||||
tossed++;
|
||||
/* printf("###: Too long packet: %d.\n", incoming_command.data_length); */
|
||||
return(TRY_AGAIN);
|
||||
}
|
||||
|
||||
// Now there must be at least incoming_command.data_length + sizeof(CRC) bytes
|
||||
// still available for us to continue.
|
||||
if ((int) PeekBytesAvail() < (incoming_command.data_length + 2)) {
|
||||
//It looked like a valid start of a packet, but it does not look
|
||||
//like the complete packet has been received yet.
|
||||
/* printf("Not enough read to check the complete message.\n"); */
|
||||
/* printf("Not enough read to check the complete message.\n"); */
|
||||
return(GIVE_UP); /* Let's not return until more byte are available */
|
||||
}
|
||||
|
||||
/* There is enough data to make a packet. Transfer over the data. */
|
||||
for(i=0;i<incoming_command.data_length;i++)
|
||||
incoming_command.data[i]=PeekByte();
|
||||
for (i = 0; i < incoming_command.data_length; i++)
|
||||
incoming_command.data[i] = PeekByte();
|
||||
|
||||
//Now move over the CRC.
|
||||
incoming_command.CRC.as_bytes[0]=PeekByte();
|
||||
incoming_command.CRC.as_bytes[1]=PeekByte();
|
||||
incoming_command.CRC.as_bytes[0] = PeekByte();
|
||||
incoming_command.CRC.as_bytes[1] = PeekByte();
|
||||
//Now check the CRC.
|
||||
|
||||
//Compute the expected CheckSum
|
||||
testcrc = get_crc((unsigned char *)&incoming_command,
|
||||
testcrc = get_crc((unsigned char *) &incoming_command,
|
||||
incoming_command.data_length+2, 0xFFFF);
|
||||
testcrc = testcrc & 0xFFFF; /* This is TRICKY */
|
||||
|
||||
if(incoming_command.CRC.as_word==testcrc)
|
||||
{
|
||||
if (incoming_command.CRC.as_word == testcrc) {
|
||||
//This is a good packet. I'll be horn swaggled. Remove the packet
|
||||
//from the serial buffer.
|
||||
AcceptPeekedData();
|
||||
//Let our caller know that incoming_command has good stuff in it.
|
||||
/* print_packet(&outgoing_response); */
|
||||
/* print_packet(&outgoing_response); */
|
||||
|
||||
return(GOOD_MSG);
|
||||
}
|
||||
@@ -509,52 +493,51 @@ unsigned char check_for_packet(int fd, unsigned char expected_length)
|
||||
*/
|
||||
tossed++;
|
||||
DiscardGetByte();
|
||||
/* printf("###: Wrong CheckSum. computed/real %04x:%04x \n",
|
||||
/* printf("###: Wrong CheckSum. computed/real %04x:%04x \n",
|
||||
testcrc, incoming_command.CRC.as_word); */
|
||||
return(TRY_AGAIN);
|
||||
}
|
||||
//============================================================================
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* I should use the value GIVE_UP and not reenter if there is no extra
|
||||
* byte readed from the serial port
|
||||
* byte read from the serial port
|
||||
*/
|
||||
int test_packet(int fd)
|
||||
{
|
||||
int is_msg;
|
||||
int is_msg;
|
||||
|
||||
is_msg=check_for_packet(fd, MAX_DATA_LENGTH);
|
||||
while (is_msg!=GIVE_UP) {
|
||||
if (is_msg==GOOD_MSG) {
|
||||
is_msg = check_for_packet(fd, MAX_DATA_LENGTH);
|
||||
while (is_msg != GIVE_UP) {
|
||||
if (is_msg == GOOD_MSG)
|
||||
treat_packet();
|
||||
|
||||
is_msg = check_for_packet(fd, MAX_DATA_LENGTH);
|
||||
}
|
||||
is_msg=check_for_packet(fd, MAX_DATA_LENGTH);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
//============================================================================
|
||||
|
||||
/*
|
||||
* This is a debuging function.
|
||||
* It should be removed or compiled in conditionaly.
|
||||
* Currently it is still using printf for debuging.
|
||||
* This is a debugging function.
|
||||
* It should be removed or compiled in conditionally.
|
||||
* Currently it is still using printf for debugging.
|
||||
*/
|
||||
|
||||
void print_packet(COMMAND_PACKET *packet)
|
||||
{
|
||||
int i, cmd, top, len;
|
||||
|
||||
top=(0xC0&(packet->command)) >> 6;
|
||||
cmd=(0x3F&(packet->command));
|
||||
len=packet->data_length;
|
||||
top = (0xC0 & (packet->command)) >> 6;
|
||||
cmd = (0x3F & (packet->command));
|
||||
len = packet->data_length;
|
||||
|
||||
printf("Message (%d,%d) %d [", top, cmd, len);
|
||||
|
||||
//There is enough data to make a packet. Transfer over the data.
|
||||
for(i=0;i<packet->data_length;i++)
|
||||
for (i = 0; i < packet->data_length; i++)
|
||||
printf(" %02x", packet->data[i]);
|
||||
|
||||
printf(" ] %02x %02x .\n", packet->CRC.as_bytes[0], packet->CRC.as_bytes[1]);
|
||||
}
|
||||
//============================================================================
|
||||
|
||||
|
||||
Reference in New Issue
Block a user