#include "UdpLightMessaging.h"
ulmSocket::ulmSocket(unsigned short port)
{
#ifdef WIN32
WSADATA socketInfo;
m_WsaInit =(WSAStartup (MAKEWORD(2,0), &socketInfo)==0);
#endif
m_hSocket = socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP);
memset(&m_BindAddr,0,sizeof(m_BindAddr));
m_BindAddr.sin_family = PF_INET;
m_BindAddr.sin_addr.s_addr = INADDR_ANY;
m_BindAddr.sin_port = htons(port);
if (bind(m_hSocket,(const struct sockaddr*)&m_BindAddr,sizeof(m_BindAddr))!=0)
fprintf(stderr,"ulm: Error binding socket to port %i\n",port);
}
ulmSocket::~ulmSocket()
{
#ifdef WIN32
closesocket(m_hSocket);
if (m_WsaInit) WSACleanup();
#else
close(m_hSocket);
#endif
}
int ulmSocket::Send(const void* buff,long size,const char* host,unsigned short port)
{
struct sockaddr_in saddr;
struct hostent *h = NULL;
#ifdef WIN32
saddr.sin_addr.s_addr=inet_addr("host");
if (saddr.sin_addr.s_addr==INADDR_NONE){
#else
if (inet_aton(host,&saddr.sin_addr)==0){
#endif
h = gethostbyname(host);
if (h==NULL) {
fprintf(stderr,"ulm: Error resolving %s\n",host);
return (-2);
}
saddr.sin_addr = *((struct in_addr *) h->h_addr);
}
saddr.sin_family = PF_INET;
saddr.sin_port = htons(port);
if(sendto(m_hSocket,(char*)buff,size,0,(const sockaddr*)&saddr, sizeof(saddr))!=size){
fprintf(stderr,"ulm: Error sending to %s\n",host);
return (-1);
}
return 0;
}
int ulmSocket::Recv(void *buff,long size,char* fromAddr,unsigned short* fromPort,long msTimeout)
{
int ret=0;
struct sockaddr_in recvAddr;
#ifdef WIN32
timeval timeOut;
timeOut.tv_sec = (long)(msTimeout/1000);
timeOut.tv_usec = (msTimeout%1000)*1000;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(m_hSocket,&readfds);
select(0, &readfds, NULL, NULL, &timeOut);
if (FD_ISSET(m_hSocket,&readfds)) {
int aDim = sizeof(recvAddr);
#else
pollfd pfd;
pfd.fd=m_hSocket;
pfd.events=POLLIN;
if (poll(&pfd,1,msTimeout)>0){
socklen_t aDim = sizeof(recvAddr);
#endif
ret = recvfrom(m_hSocket,(char*)buff,size,0,(struct sockaddr*)&recvAddr,&aDim);
}
if ((ret>0)&&(fromAddr!=NULL)) strncpy(fromAddr,inet_ntoa(recvAddr.sin_addr),16);
if ((ret>0)&&(fromPort!=NULL)) *fromPort = ntohs(recvAddr.sin_port);
return ret;
}
ulmMessage::ulmMessage(char ch1,char ch2,char ch3)
{
m_ch1=ch1;
m_ch2=ch2;
m_ch3=ch3;
Clear();
m_binFlag=false;
}
ulmMessage::~ulmMessage(){}
int ulmMessage::FieldAdd(const char* label, const char* value)
{
if (m_binFlag) return -2;
if ((strlen(label)+strlen(value)+msg_dim+2)>ULM_MAX_MSG) return -1;
*(msg_buf+msg_dim+ULM_HEAD_DIM)=0;
strcpy((msg_buf+msg_dim+ULM_HEAD_DIM),label);
msg_dim += strlen(label) + 1;
*(msg_buf+msg_dim+ULM_HEAD_DIM)=0;
strcpy((msg_buf+msg_dim+ULM_HEAD_DIM),value);
msg_dim += strlen(value) + 1;
return 0;
}
int ulmMessage::FieldGet(const char* label, char* value, long valueDimBytes)
{
*value=0;
char *pN,*pV;
pN=msg_buf+ULM_HEAD_DIM;
pV=msg_buf+ULM_HEAD_DIM;
while((pV<(msg_buf+msg_dim+ULM_HEAD_DIM))&&(*pV!=0)&&(*pV!=27)) pV++;
if (*pV==0) pV++;
else return -1;
while(pV<(msg_buf+msg_dim+ULM_HEAD_DIM)){
if (!strcmp(pN,label)){
strncpy(value,pV,valueDimBytes);
return 0;
}
pN=pV;
while((pN<(msg_buf+msg_dim+ULM_HEAD_DIM))&&(*pN!=0)&&(*pN!=27)) pN++;
if (*pN==0) pN++;
else return -1;
pV=pN;
while((pV<(msg_buf+msg_dim+ULM_HEAD_DIM))&&(*pV!=0)&&(*pN!=27)) pV++;
if (*pV==0) pV++;
else return -1;
}
return -1;
}
int ulmMessage::BinaryFieldAdd(const char* label,const void* data,unsigned short size)
{
if ((msg_dim+strlen(label)+4+size)>ULM_MAX_MSG) return -1;
*(msg_buf+msg_dim+ULM_HEAD_DIM)=27; *(msg_buf+msg_dim+ULM_HEAD_DIM+1)=0; strcpy((msg_buf+msg_dim+ULM_HEAD_DIM+1),label); *((unsigned short*)(msg_buf+msg_dim+ULM_HEAD_DIM+strlen(label)+2)) = size; memcpy((msg_buf+ULM_HEAD_DIM+msg_dim+strlen(label)+4),data,size); msg_dim += strlen(label)+size+4;
m_binFlag = true;
return 0;
}
int ulmMessage::BinaryFieldGet(const char* label,void* data,unsigned short maxsize,unsigned short* size)
{
char *pD;
pD=msg_buf+ULM_HEAD_DIM;
*size = 0;
while(pD<(msg_buf+msg_dim+ULM_HEAD_DIM)){
while((pD<(msg_buf+msg_dim+ULM_HEAD_DIM))&&(*pD!=27)) pD++;
if (*pD==27) pD++;
else return -1;
*size = *((unsigned short*)(pD+strlen(pD)+1));
if (!strcmp(label,pD)){
memcpy(data,(pD+strlen(pD)+3),maxsize);
return 0;
}
pD += strlen(label)+*size+3;
}
return -1;
}
int ulmMessage::Send(unsigned short lport,const char* host,unsigned short port)
{
ulmSocket OutSock(lport);
msg_buf[0]=m_ch1;
msg_buf[1]=m_ch2;
msg_buf[2]=m_ch3;
msg_buf[3]=(unsigned char)(msg_dim%256);
msg_buf[4]=(unsigned char)(msg_dim/256);
msg_buf[5]=0;
msg_buf[6]=0;
msg_buf[7]=0;
msg_buf[8]=0;
msg_buf[9]=0;
msg_buf[10]=0;
msg_buf[11]=computeXOR(msg_buf+ULM_HEAD_DIM,msg_dim);
msg_buf[12]=computeXOR(msg_buf,ULM_HEAD_DIM-1);
return (OutSock.Send(msg_buf,msg_dim+ULM_HEAD_DIM,host,port));
}
void ulmMessage::Clear()
{
msg_dim=0;
memset(msg_buf,0,ULM_MAX_MSG+ULM_HEAD_DIM);
m_binFlag = false;
}
char ulmMessage::computeXOR(char* buf,long dim)
{
char ret=0;
for ( int i=0 ; i<dim ; i++ ) ret=ret^*(buf+i);
return ret;
}
ulmReceiver::ulmReceiver(void* pClient,ulmCallback* Cb,unsigned short LocalPort,
char ch1,char ch2,char ch3)
{
m_RTparms.pClient = pClient;
m_RTparms.ClientCb = Cb;
m_RTparms.Socket = new ulmSocket(LocalPort);
m_RTparms.ch1=ch1;
m_RTparms.ch2=ch2;
m_RTparms.ch3=ch3;
if (pthread_create(&m_pRecvThread,NULL,&RecvThread,(void*)&m_RTparms)!=0)
fprintf(stderr,"ulm: Error creating receiver thread\n");
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
}
ulmReceiver::~ulmReceiver()
{
if (pthread_cancel(m_pRecvThread)!=0)
fprintf(stderr,"ulm: Error cancelling receiver thread\n");
if (pthread_join(m_pRecvThread,NULL)!=0)
fprintf(stderr,"ulm: Error joining receiver thread\n");
delete m_RTparms.Socket;
}
void* ulmReceiver::RecvThread(void* Parms)
{
ulmMessage InMsg;
char fromAddr[16];
unsigned short fromPort;
ulmSocket* SockIn = ((struct RecvThreadParms*)Parms)->Socket;
ulmCallback* ClientCb = ((struct RecvThreadParms*)Parms)->ClientCb;
void* pClient = ((struct RecvThreadParms*)Parms)->pClient;
char ch1 = ((struct RecvThreadParms*)Parms)->ch1;
char ch2 = ((struct RecvThreadParms*)Parms)->ch2;
char ch3 = ((struct RecvThreadParms*)Parms)->ch3;
bool ignorehead=true;
if (ch1|ch2|ch3 != 0) ignorehead=false;
while (1){
if (SockIn->Recv(InMsg.msg_buf,ULM_MAX_MSG+ULM_HEAD_DIM,fromAddr,&fromPort)>0){
if ((ignorehead)||
((InMsg.msg_buf[0]==ch1)&&
(InMsg.msg_buf[1]==ch2)&&
(InMsg.msg_buf[2]==ch3))){
InMsg.msg_dim=(unsigned char)InMsg.msg_buf[3]+(unsigned char)InMsg.msg_buf[4]*256;
ClientCb(InMsg,pClient);
}
InMsg.Clear();
}
pthread_testcancel();
}
return NULL;
}