From 99c7d32aa78694885de597473793777b10b1c47f Mon Sep 17 00:00:00 2001 From: Stefan `Sec` Zehl Date: Wed, 30 May 2012 02:27:47 +0200 Subject: [PATCH] first draft of an openbeacon reader written in C --- tools/reader/obreader.c | 279 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 279 insertions(+) create mode 100644 tools/reader/obreader.c diff --git a/tools/reader/obreader.c b/tools/reader/obreader.c new file mode 100644 index 0000000..1749dd0 --- /dev/null +++ b/tools/reader/obreader.c @@ -0,0 +1,279 @@ +/* obreader.c by Sec + * vim:set cin sm ts=4: + * + * Do whatever you want with this code, but give credit + * + * This program reads packets from an USB-Serial R0ket + * and sends them off via TCP/UDP to a central host + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define BUFSIZE 100 +#define PORT 2342 +#define SRV_IP "127.0.0.1" + +/* rotating buffer */ +#define ELEMSIZE 32 +unsigned int bufstart=0,bufend=0; /* rotating buffer */ +#define BUFIDX(a) (a*ELEMSIZE) +#define BUFPUSH() do{ bufend++; if(bufend==BUFSIZE){ bufend=0; }; if(bufend==bufstart){ BUFPOP();}; }while(0) +#define BUFPOP() do{ bufstart++; if(bufstart==BUFSIZE){ bufstart=0; }; }while(0) +unsigned char buffer[BUFIDX(BUFSIZE)]; + +static u_int16_t +crc16 (const unsigned char *buffer, int size) +{ + u_int16_t crc = 0xFFFF; + + if (buffer && size) + while (size--) + { + crc = (crc >> 8) | (crc << 8); + crc ^= *buffer++; + crc ^= ((unsigned char) crc) >> 4; + crc ^= crc << 12; + crc ^= (crc & 0xFF) << 5; + } + + return crc; +} + +void setnonblocking(int fd) { + int opts; + + opts = fcntl(fd,F_GETFL); + if (opts < 0) { + perror("fcntl(F_GETFL)"); + exit(EXIT_FAILURE); + } + opts = (opts | O_NONBLOCK); + if (fcntl(fd,F_SETFL,opts) < 0) { + perror("fcntl(F_SETFL)"); + exit(EXIT_FAILURE); + } + return; +} + +/* 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_int16_t*)(buffer+BUFIDX(idx)+0)=htons(0xffff ^ crc16(buffer+BUFIDX(idx)+2,30)); +} + +void read_r0ket(int fd){ + int r,t,o,x; + static unsigned char data[64]; + static unsigned char offset=0; + static unsigned char firstread=1; + + r=read(fd,data+offset,sizeof(data)-offset); + if(r<0){ + perror("read(device)"); + exit(EXIT_FAILURE); + }; + if(r==0){ + printf("nothing read. Shouldn't happen\n"); + return; + }; +#if 0 + for(t=offset;t0){ + if(!firstread) + printf("ignoring garbage: %02X\n",data[0]); + memmove(data,data+1,sizeof(data)-1); + r--; + }; + if(r==0){ /* no data left */ + return; + }; + + if(data[1]=='1'){ + firstread=0; + /* find frame end */ + for(x=2;x=r-1){ /* no EOF found */ + if(r>60){ + printf("serial frame content overflow\n"); + return; + }; + offset=r; /* keep unused data for next round */ + return; + }; + +// printf("consume %d: ",x); + o=16; + BUFPUSH(); + for(t=2;t=ELEMSIZE) /* "buffer" overflow protection */ + break; + buffer[BUFIDX(bufend)+o]=data[t]; + if(data[t]!='\\') + o++; +// printf("%02x",data[t]); + }; + pkt_cleanup(bufend); + x+=2; /* also consume end of frame marker */ +// printf("\n"); + }else if(data[1]=='7'){ /* beaconid frame */ + /* 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)); + x=8; + }else if(data[1]=='2'){ /* command ack frame */ + x=4; /* just consume, and do nothing */ + }else{ + if(!firstread) + printf("invalid frame type: %02x\n",data[1]); + x=2; + }; + if(x==r) /* all data consumed */ + return; + /* keep unconsumed data */ + memmove(data,data+x,r-x); + offset=r-x; + return; +} + +void write_socket(int sockfd){ + BUFPOP(); + if (send(sockfd, buffer+BUFIDX(bufstart), ELEMSIZE, 0)==-1){ + perror("send"); + exit(EXIT_FAILURE); + }; +} + +int main(int argc, char ** argv) +{ + int c; /* getopt return value */ + char *device="/dev/ttyACM0"; + int devfd,sockfd; /* FD for device & socket */ + int maxfd=0; + int t; + fd_set rset,wset,eset; /* file descriptors for select() */ + struct timeval timeout; /* Timeout for select */ + struct sockaddr_in si_other; /* target socket */ + + /* The big getopt loop */ + while ((c = getopt(argc, argv, "d:")) != EOF) + switch (c) + { + case 'd': + device=(char *)malloc(strlen(optarg)+2); + strcpy(device,optarg); + break; + + default: + fprintf(stderr, "Usage: %s [options] \n\n\ +This program reads packets from an USB-Serial R0ket\n\ +and sends them off via TCP/UDP to a central host\n\n\ + -d Open a different device instead of '%s'\n\ + -h This help\n", + argv[0], + device + ); + exit(255); + } + +/* argc -= optind; argv += optind; *//* only if we want more args */ + + /* Open & prep input device */ + if((devfd=open(device,O_RDWR)) == -1) + perror("open_device"); + + setnonblocking(devfd); + + /* Open & prep outout device */ + if ((sockfd=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1){ + perror("socket"); + exit(EXIT_FAILURE); + }; + memset((char *) &si_other, 0, sizeof(si_other)); + si_other.sin_family = AF_INET; + si_other.sin_port = htons(PORT); + if (inet_aton(SRV_IP, &si_other.sin_addr)==0) { + perror("inet_aton()"); + exit(EXIT_FAILURE); + } + if(connect(sockfd,(struct sockaddr*)&si_other,sizeof(si_other))<0){ + perror("connect"); + exit(EXIT_FAILURE); + }; + setnonblocking(sockfd); + + /* prepare stuff for select */ + if(devfd>maxfd) + maxfd=devfd; + if(sockfd>maxfd) + maxfd=sockfd; + + while(1){ + timeout.tv_sec = 1; + timeout.tv_usec = 0; + + FD_ZERO(&rset); + FD_SET(devfd,&rset); + FD_ZERO(&wset); + if(bufstart!=bufend){ + FD_SET(sockfd,&wset); + }; + FD_ZERO(&eset); + FD_SET(devfd,&eset); + + t = select(maxfd+1, &rset, &wset, &eset, &timeout); + + if (t<0){ + perror("select"); + exit(EXIT_FAILURE); + }; + + if (t==0){ /* timeout */ + printf("[timeout]\n"); + }; + + if (FD_ISSET(devfd,&rset)) + read_r0ket(devfd); + + if (FD_ISSET(sockfd,&wset)) + write_socket(sockfd); + + }; + return(0); +}