diff --git a/tools/reader/obreader.c b/tools/reader/obreader.c index 1749dd0..3c27997 100644 --- a/tools/reader/obreader.c +++ b/tools/reader/obreader.c @@ -19,10 +19,15 @@ #include #include +#define INTERVAL 1 + #define BUFSIZE 100 #define PORT 2342 #define SRV_IP "127.0.0.1" +#define TYPE_UDP +#undef TYPE_TCP + /* rotating buffer */ #define ELEMSIZE 32 unsigned int bufstart=0,bufend=0; /* rotating buffer */ @@ -31,6 +36,8 @@ unsigned int bufstart=0,bufend=0; /* rotating buffer */ #define BUFPOP() do{ bufstart++; if(bufstart==BUFSIZE){ bufstart=0; }; }while(0) unsigned char buffer[BUFIDX(BUFSIZE)]; +time_t the_time; + static u_int16_t crc16 (const unsigned char *buffer, int size) { @@ -68,20 +75,58 @@ void setnonblocking(int fd) { /* Reference is https://r0ket.badge.events.ccc.de/tracking:reader */ void pkt_cleanup(int idx){ static u_int32_t ctr; - time_t t; - time(&t); buffer[BUFIDX(idx)+2]=1; // BEACONLOG_SIGHTING buffer[BUFIDX(idx)+3]=0; // interface 0 *(u_int16_t*)(buffer+BUFIDX(idx)+4)=htons(1234); // reader id *(u_int16_t*)(buffer+BUFIDX(idx)+6)=htons(32); // size *(u_int32_t*)(buffer+BUFIDX(idx)+8)=htonl(ctr++); - *(u_int32_t*)(buffer+BUFIDX(idx)+12)=htonl(t); + *(u_int32_t*)(buffer+BUFIDX(idx)+12)=htonl(the_time); *(u_int16_t*)(buffer+BUFIDX(idx)+0)=htons(0xffff ^ crc16(buffer+BUFIDX(idx)+2,30)); } -void read_r0ket(int fd){ +void write_r0ket(int fd,char * buf,int len){ + int r; + r=write(fd,buf,len); + if(r!=len){ + printf("wrote only %d bytes of %d to device\n",r,len); + exit(EXIT_FAILURE); + }; +} + +void setup_r0ket(int fd){ + unsigned char buf[256]; + int x,r; + u_int32_t uuid=0; + + write_r0ket(fd,"\\7\\0",4); /* Get UUID */ + write_r0ket(fd,"\\4\001\002\003\002\001\\0",9); /* Set rx_mac */ + write_r0ket(fd,"\\5\x51\\0",5); /* Set channel */ + write_r0ket(fd,"\\6\x10\\0",5); /* Set rx_len */ + + usleep(100000); /* wait 100ms for an answer */ + r=read(fd,buf,sizeof(buf)); + /* try to find uuid in reply. Could be nicer, but it works. */ + if(r>7){ + for(x=0;x=r-1){ /* no EOF found */ if(r>60){ printf("serial frame content overflow\n"); - return; + return 0; }; offset=r; /* keep unused data for next round */ - return; + return 0; }; // printf("consume %d: ",x); @@ -147,15 +193,28 @@ void read_r0ket(int fd){ x+=2; /* also consume end of frame marker */ // printf("\n"); }else if(data[1]=='7'){ /* beaconid frame */ + /* find frame end */ + for(x=2;x=r-1){ /* no EOF found */ + if(r>60){ + printf("serial frame content overflow\n"); + return 0; + }; + offset=r; /* keep unused data for next round */ + return 0; + }; /* XXX: do something with beaconid */ BUFPUSH(); for(t=0;t<16;t++){ /* clear buffer */ buffer[BUFIDX(bufend)+16+t]=0; }; buffer[BUFIDX(bufend)+16]=22; // RFBPROTO_READER_ANNOUNCE - *(u_int16_t*)(buffer+BUFIDX(bufend)+14)=0; - *(u_int16_t*)(buffer+BUFIDX(bufend)+14)= \ - htons(crc16(buffer+BUFIDX(bufend),14)); + *(u_int16_t*)(buffer+BUFIDX(bufend)+16+14)= \ + htons(crc16(buffer+BUFIDX(bufend)+16,14)); + pkt_cleanup(bufend); x=8; }else if(data[1]=='2'){ /* command ack frame */ x=4; /* just consume, and do nothing */ @@ -165,31 +224,33 @@ void read_r0ket(int fd){ x=2; }; if(x==r) /* all data consumed */ - return; + return 0; /* keep unconsumed data */ memmove(data,data+x,r-x); offset=r-x; - return; + return 0; } void write_socket(int sockfd){ BUFPOP(); if (send(sockfd, buffer+BUFIDX(bufstart), ELEMSIZE, 0)==-1){ perror("send"); - exit(EXIT_FAILURE); +// exit(EXIT_FAILURE); }; } -int main(int argc, char ** argv) -{ +int main(int argc, char ** argv){ int c; /* getopt return value */ char *device="/dev/ttyACM0"; - int devfd,sockfd; /* FD for device & socket */ + int devfd=-1,sockfd=-1,listenfd=-1; /* FD for device & socket */ int maxfd=0; - int t; - fd_set rset,wset,eset; /* file descriptors for select() */ - struct timeval timeout; /* Timeout for select */ + int cnt; + fd_set rset,wset; /* file descriptors for select() */ + struct timeval timeout; /* Timeout for select */ struct sockaddr_in si_other; /* target socket */ + time_t ot,heartbeat=0; + + time(&ot); /* The big getopt loop */ while ((c = getopt(argc, argv, "d:")) != EOF) @@ -215,14 +276,17 @@ and sends them off via TCP/UDP to a central host\n\n\ /* argc -= optind; argv += optind; *//* only if we want more args */ /* Open & prep input device */ - if((devfd=open(device,O_RDWR)) == -1) - perror("open_device"); - + if((devfd=open(device,O_RDWR)) == -1){ + perror("open(device)"); + exit(EXIT_FAILURE); + }; setnonblocking(devfd); + setup_r0ket(devfd); /* Open & prep outout device */ +#ifdef TYPE_UDP if ((sockfd=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1){ - perror("socket"); + perror("socket(udp)"); exit(EXIT_FAILURE); }; memset((char *) &si_other, 0, sizeof(si_other)); @@ -237,43 +301,139 @@ and sends them off via TCP/UDP to a central host\n\n\ exit(EXIT_FAILURE); }; setnonblocking(sockfd); - - /* prepare stuff for select */ - if(devfd>maxfd) - maxfd=devfd; - if(sockfd>maxfd) - maxfd=sockfd; +#endif +#ifdef TYPE_TCP + if((listenfd = socket(AF_INET, SOCK_STREAM, 0))<0){ + perror("socket(tcpserver)"); + exit(EXIT_FAILURE); + }; + memset((char *) &si_other, 0, sizeof(si_other)); + si_other.sin_family = AF_INET; + si_other.sin_addr.s_addr = INADDR_ANY; + si_other.sin_port = htons(PORT); + if(bind(listenfd,(struct sockaddr*)&si_other,sizeof(si_other))<0){ + perror("bind"); + exit(EXIT_FAILURE); + }; + if(listen(listenfd,1)!=0){ + perror("listen"); + exit(EXIT_FAILURE); + }; +#endif while(1){ + /* prepare stuff for select */ timeout.tv_sec = 1; timeout.tv_usec = 0; + if(devfd>maxfd) + maxfd=devfd; + if(sockfd>maxfd) + maxfd=sockfd; + if(listenfd>maxfd) + maxfd=listenfd; + FD_ZERO(&rset); + if(devfd==-1){ + fprintf(stderr,"Can't yet deal with disappearing device\n"); + exit(EXIT_FAILURE); + }; FD_SET(devfd,&rset); + if(sockfd!=-1){ +#ifdef TYPE_TCP + FD_SET(sockfd,&rset); +#endif + }else if(listenfd!=-1){ + FD_SET(listenfd,&rset); + }; + FD_ZERO(&wset); - if(bufstart!=bufend){ + if(bufstart!=bufend && sockfd!=-1){ FD_SET(sockfd,&wset); }; - FD_ZERO(&eset); - FD_SET(devfd,&eset); - t = select(maxfd+1, &rset, &wset, &eset, &timeout); + cnt = select(maxfd+1, &rset, &wset, NULL, &timeout); - if (t<0){ + + /* First run timer stuff */ + time(&the_time); + if(the_time-ot>=INTERVAL){ + ot=the_time; + printf("[running: buf=%d, sockfd=%d]\n",(BUFSIZE+bufend-bufstart)%BUFSIZE,sockfd); + }; + + if(the_time-heartbeat>=1){ + heartbeat=the_time; + write_r0ket(devfd,"\\7\\0",4); /* Get UUID */ + }; + + /* Now check select / fds*/ + if (cnt<0){ perror("select"); exit(EXIT_FAILURE); }; - if (t==0){ /* timeout */ + if (cnt==0){ /* timeout */ printf("[timeout]\n"); + continue; }; - if (FD_ISSET(devfd,&rset)) - read_r0ket(devfd); + if (FD_ISSET(devfd,&rset)){ + if(read_r0ket(devfd)<0){ + close(devfd); + devfd=-1; + }; + if(--cnt ==0) continue; + }; - if (FD_ISSET(sockfd,&wset)) + if (sockfd!=-1 && FD_ISSET(sockfd,&rset)){ + int r; + unsigned char dummy[32]; + r=read(sockfd,dummy,32); + if(r<0){ + perror("read(socket)"); + exit(EXIT_FAILURE); + }; + if(r==0){ + printf("eof() on socket\n"); + close(sockfd); + sockfd=-1; + }; + if(r>0){ + printf("read [%d] bytes from socket and ignored them.\n",r); + }; + + if(--cnt ==0) continue; + }; + + if (sockfd!=-1 && FD_ISSET(sockfd,&wset)){ write_socket(sockfd); + if(--cnt ==0) continue; + }; + if (listenfd!=-1 && FD_ISSET(listenfd,&rset)){ + if(sockfd!=-1){ // close old connection + close(sockfd); + }; + unsigned int size=sizeof(si_other); + if((sockfd=accept(listenfd,(struct sockaddr*)&si_other,&size))<0){ + perror("accept"); + continue; // Do not exit, we can handle this :-) + }; + printf("New connection from %s (fd %d)\n", inet_ntoa(si_other.sin_addr), sockfd); + if(--cnt ==0) continue; + }; + + printf("unknwon select left over: cnt=%d, ",cnt); + printf("rset: "); + for(cnt=0;cnt