#include <string.h>
char strout[100], recv[100], recv_hd[100];
unsigned short slaveAdd, func, nb, startAdd;
unsigned short crc;
float result[5];
char *pt;
do_cyc(unsigned short data)
{
int i;
unsigned short tmp;
crc = data ^ crc;
for (i=0 ; i<8 ; i++)
{
tmp = crc & (0x0001);
crc = crc >> 1;
if (tmp == 1)
{
crc = 0xa001 ^ crc;
}
}
}
unsigned short get_crc()
{
crc = 0xffff;
do_cyc(slaveAdd & 0x00ff);
do_cyc(func & 0x00ff);
do_cyc((startAdd & 0xff00) >> 8);
do_cyc(startAdd & 0x00ff);
do_cyc((nb & 0xff00) >> 8);
do_cyc(nb & 0x00ff);
unsigned short swap;
swap = crc;
crc = (crc >> 8) & 0x00ff;
crc = crc | ((swap << 8) & 0xff00);
}
float conv(int data)
{
float ret;
memcpy(&ret, &data, sizeof(int));
return ret;
}
void pre_handle()
{
int i, data;
pt = recv;
crc = 0xffff;
strncpy(recv_hd, pt, 2);
recv_hd[2] = '\0';
sscanf(recv_hd, "%hX", &slaveAdd);
pt += 2;
do_cyc(slaveAdd & 0x00ff);
strncpy(recv_hd, pt, 2);
recv_hd[2] = '\0';
sscanf(recv_hd, "%hX", &func);
pt += 2;
do_cyc(func & 0x00ff);
strncpy(recv_hd, pt, 2);
recv_hd[2] = '\0';
sscanf(recv_hd, "%hX", &nb);
pt += 2;
do_cyc(nb & 0x00ff);
for (i=0 ; i<nb/4 ; i++)
{
strncpy(recv_hd, pt, 8);
sscanf(recv_hd, "%X", &data);
result[i] = conv(data);
do_cyc((unsigned short)((data & 0xff000000) >> 24));
do_cyc((unsigned short)((data & 0x00ff0000) >> 16));
do_cyc((unsigned short)((data & 0x0000ff00) >> 8));
do_cyc((unsigned short)(data & 0x000000ff));
pt += 8;
}
unsigned short swap;
swap = crc;
crc = (crc >> 8) & 0x00ff;
crc = crc | ((swap << 8) & 0xff00);
}
int main(int argc, char *argv[])
{
char cmp[10];
while (EOF != scanf("%hd,%hd,%hd,%hd", &slaveAdd, &func, &startAdd, &nb))
{
get_crc();
sprintf(strout, "%02X%02X%04X%04X%04X", slaveAdd, func, startAdd, nb, crc);
printf("%s\n", strout);
scanf("%s", recv);
pre_handle(recv);
sprintf(cmp, "%hX", crc);
if (0 != strcmp(cmp, pt))
{
printf("CRC_ERROR\n");
continue;
}
int i;
for (i=0 ; i<nb/4-1 ; i++)
printf("%.1f,", result[i]);
printf("%.1f\n", result[i]);
}
}
|