crashtest-r0ket/tools/reader/obreader.c

280 lines
6.9 KiB
C

/* obreader.c by Sec <sec@42.org>
* 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 <sys/types.h>
#include <arpa/inet.h>
#include <fcntl.h>
#include <sys/uio.h>
#include <unistd.h>
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <pwd.h>
#include <stdlib.h>
#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;t<offset+r;t++){
printf("%02x",data[t]);
};
printf (" read %d bytes, offset was %d\n",r,offset);
#endif
r+=offset;
offset=0;
/* find frame start */
while(data[0] != '\\' && r>0){
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;x++){
if(data[x]=='\\' && data[x+1]=='0')
break;
}
if(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<x;t++){
if(o>=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 <device> 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);
}