int collect_process(struct collect_state *cs, gnuplot_ctrl *bsln_disp[4], int *real_raw, int *imag_raw)
{
struct header_poco *hp;
struct option_poco *op;
unsigned int options, base, payload, i;
int wr, or, result;
int j, k;
unsigned int value, compare, flag;
result = 0;
compare = 0;
if(cs->c_have < 8){
fprintf(stderr, "process: short packet (len=%u)\n", cs->c_have);
return -1;
}
hp = (struct header_poco *) cs->c_buffer;
if(hp->h_magic != htons(MAGIC_HEADER_POCO)){
fprintf(stderr, "process: bad header magic 0x%04x\n", hp->h_magic);
cs->c_errors++;
return -1;
}
if(hp->h_version != htons(VERSION_HEADER_POCO)){
fprintf(stderr, "process: odd version %d\n", ntohs(hp->h_version));
cs->c_errors++;
return -1;
}
options = ntohs(hp->h_options);
base = (options + 1) * 8;
if(base > cs->c_have){
fprintf(stderr, "process: options larger than packet itself\n");
cs->c_errors++;
return -1;
}
payload = cs->c_have - base;
if(cs->c_meta){
fprintf(cs->c_meta, "frame %lu: options %u, length %u, payload %u\n", cs->c_rxp, options, cs->c_have, payload);
}
for(i = 0; i < options; i++){
op = (struct option_poco *)(cs->c_buffer + ((i + 1) * 8));
or = option_process(cs, op);
if(or){
result = or;
}
}
cs->c_rxopts += options;
/* at this point we have the number of data bytes in payload, and the start of data at at cp->c_bufer + base */
if(payload){
wr = write(cs->c_ffd, cs->c_buffer + base, payload);
if(wr < payload){
fprintf(stderr, "process: unable to write payload of %d\n", payload);
return -1;
}
cs->c_rxdata += payload;
/*This is lame but i think it works*/
if(!cs->c_offset){
cs->c_tracker = 0;
cs->c_tracker += payload;
}
else{
cs->c_tracker += payload;
}
/*Check for lost packets*/
if((cs->c_tracker-payload) != cs->c_offset){
cs->c_lostpackets++;
cs->lost_flag = 1;
}
fprintf(stderr, "%lu %lu %d->%d\n", cs->c_rxdata, (cs->c_tracker-payload) , cs->c_offset, cs->lost_flag);
if(cs->c_text != NULL){
/* TODO: maybe write data out in human readable form to cs->c_text ? */
/* fprintf(cs->c_text, "..."); */
if(cs->c_new){
collect_precalculate(cs);
cs->c_new = 0;
}
#ifdef DEBUG
fprintf(stderr, "base=%d, payload=%d, size=%d\n", base, payload, cs->c_size);
#endif
for(j = base; j < (base + payload); j += cs->c_size){
value = 0;
compare = cs->c_buffer[base + cs->c_extra];
if(compare & 0x80){
flag = 1;
}
for(k = 0; k < cs->c_size; k++){
value *= 256;
value += cs->c_buffer[j + cs->c_extra + (cs->c_direction * k)];
}
/*Check for data type:unsigned,signed or float*/
if(!cs->c_datatype){
fprintf(cs->c_text,"%u%c", value, (cs->c_cnt == 0 || cs->c_cnt % 23 != 0 ) ? ',' : '\n');
}
else if(cs->c_datatype == 1){
if(flag){
value = (value | cs->c_mask);
}
if((cs->c_cnt % 2) == 0){
real_raw[(cs->c_counter * 12) + (cs->c_cnt / 2)] = value;
}
else{
imag_raw[(cs->c_counter * 12) + (cs->c_cnt / 2)] = value;
}
fprintf(cs->c_text,"%d%c", value, (cs->c_cnt == 0 || cs->c_cnt % 23 != 0) ? ',' : '\n');
}
else{
/*TODO:Floating point case*/
}
flag = 0;
cs->c_cnt++;
if(cs->c_cnt == 24){
cs->c_cnt = 0;
cs->c_counter++;
}
if(cs->c_counter == 512 ){
if(!cs->lost_flag){
/*Display real time power and phase plots*/
if(display_baseline(cs->c_baseline, bsln_disp, real_raw, imag_raw) < 0){
fprintf(stderr,"main: error getting and displaying baseline %d\n", cs->c_baseline);
return -1;
}
}
cs->c_counter = 0;
cs->lost_flag = 0;
}
}
}
}
return result;
}
int collect_process(struct collect_state *cs)
{
struct header_poco *hp;
struct option_poco *op;
unsigned int options, base, payload, i;
int wr, or, result;
int j, k;
unsigned int value, compare, flag;
unsigned int cnt = 1;
result = 0;
compare = 0;
if(cs->c_have < 8){
fprintf(stderr, "process: short packet (len=%u)\n", cs->c_have);
return -1;
}
hp = (struct header_poco *) cs->c_buffer;
if(hp->h_magic != htons(MAGIC_HEADER_POCO)){
fprintf(stderr, "process: bad header magic 0x%04x\n", hp->h_magic);
cs->c_errors++;
return -1;
}
if(hp->h_version != htons(VERSION_HEADER_POCO)){
fprintf(stderr, "process: odd version %d\n", ntohs(hp->h_version));
cs->c_errors++;
return -1;
}
options = ntohs(hp->h_options);
base = (options + 1) * 8;
if(base > cs->c_have){
fprintf(stderr, "process: options larger than packet itself\n");
cs->c_errors++;
return -1;
}
payload = cs->c_have - base;
if(cs->c_meta){
fprintf(cs->c_meta, "frame %lu: options %u, length %u, payload %u\n", cs->c_rxp, options, cs->c_have, payload);
}
for(i = 0; i < options; i++){
op = (struct option_poco *)(cs->c_buffer + ((i + 1) * 8));
or = option_process(cs, op);
if(or){
result = or;
}
}
cs->c_rxopts += options;
/* at this point we have the number of data bytes in payload, and the start of data at at cp->c_bufer + base */
if(payload){
wr = write(cs->c_ffd, cs->c_buffer + base, payload);
if(wr < payload){
fprintf(stderr, "process: unable to write payload of %d\n", payload);
return -1;
}
cs->c_rxdata += payload;
if(cs->c_text != NULL){
/* TODO: maybe write data out in human readable form to cs->c_text ? */
/* fprintf(cs->c_text, "..."); */
if(cs->c_new){
collect_precalculate(cs);
cs->c_new = 0;
}
#ifdef DEBUG
fprintf(stderr, "base=%d, payload=%d, size=%d\n", base, payload, cs->c_size);
#endif
for(j = base; j < (base + payload); j += cs->c_size){
value = 0;
compare = cs->c_buffer[base + cs->c_extra];
if(compare & 0x80){
flag = 1;
}
for(k = 0; k < cs->c_size; k++){
value *= 256;
value += cs->c_buffer[j + cs->c_extra + (cs->c_direction * k)];
}
/*Check for data type:unsigned,signed or float*/
if(!cs->c_datatype){
fprintf(cs->c_text,"%u%c", value, (cnt % 24 != 0) ? ' ' : '\n');
}
else if(cs->c_datatype == 1){
if(flag){
value = (value | cs->c_mask);
}
fprintf(cs->c_text,"%d%c", value, (cnt % 24 != 0) ? ' ' : '\n');
}
else{
/*TODO:Floating point case*/
}
flag = 0;
cnt++;
if(cnt == 24){
cnt = 0;
}
}
}
}
return result;
}