first draft of an openbeacon reader written in C
This commit is contained in:
Stefan `Sec` Zehl
parent
a2ac676134
commit
99c7d32aa7
|
|
@ -0,0 +1,279 @@
|
|||
/* 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);
|
||||
}
|
||||
Loading…
Reference in New Issue