A multi-interface listener, threaded approach to UDPWii
git-svn-id: https://dolphin-emu.googlecode.com/svn/trunk@5821 8ced0084-cf51-0410-be5f-012b33b47a6e
This commit is contained in:
dapetcu21
parent
12a676c273
commit
0dc8833396
|
|
@ -385,9 +385,7 @@ void FillReportInfo(wm_core& _core)
|
|||
if (!IsFocus()) return;
|
||||
|
||||
u32 mask=0;
|
||||
if (WiiMapping[g_ID].UDPWM.instance)
|
||||
WiiMapping[g_ID].UDPWM.instance->update();
|
||||
|
||||
|
||||
if ((WiiMapping[g_ID].UDPWM.instance)&&(WiiMapping[g_ID].UDPWM.enableButtons))
|
||||
mask=WiiMapping[g_ID].UDPWM.instance->getButtons();
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,6 @@
|
|||
|
||||
#include <winsock2.h>
|
||||
#include <ws2tcpip.h>
|
||||
|
||||
#define sock_t SOCKET
|
||||
#define ERRNO WSAGetLastError()
|
||||
#define EWOULDBLOCK WSAEWOULDBLOCK
|
||||
|
|
@ -38,24 +37,44 @@
|
|||
|
||||
#endif
|
||||
|
||||
#include "Thread.h"
|
||||
#include "EmuMain.h"
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <list>
|
||||
|
||||
struct UDPWiimote::_d
|
||||
{
|
||||
sock_t sockfd;
|
||||
Common::Thread * thread;
|
||||
std::list<sock_t> sockfds;
|
||||
Common::CriticalSection termLock,mutex;
|
||||
volatile bool exit;
|
||||
};
|
||||
|
||||
int UDPWiimote::noinst=0;
|
||||
|
||||
UDPWiimote::UDPWiimote(const char *port) : d(new _d) ,x(0),y(0),z(0),nunX(0),nunY(0),pointerX(-0.1),pointerY(-0.1),nunMask(0),mask(0)
|
||||
void _UDPWiiThread(void* arg)
|
||||
{
|
||||
((UDPWiimote*)arg)->mainThread();
|
||||
//NOTICE_LOG(WIIMOTE,"UDPWii thread stopped");
|
||||
}
|
||||
|
||||
THREAD_RETURN UDPWiiThread(void* arg)
|
||||
{
|
||||
_UDPWiiThread(arg);
|
||||
return 0;
|
||||
}
|
||||
|
||||
UDPWiimote::UDPWiimote(const char *port) :
|
||||
d(new _d) ,x(0),y(0),z(0),nunX(0),nunY(0),
|
||||
pointerX(-0.1),pointerY(-0.1),nunMask(0),mask(0),time(0)
|
||||
{
|
||||
#ifdef _WIN32
|
||||
u_long iMode = 1;
|
||||
#endif
|
||||
struct addrinfo hints, *servinfo, *p;
|
||||
int rv;
|
||||
d->thread=NULL;
|
||||
|
||||
#ifdef _WIN32
|
||||
if (noinst==0)
|
||||
|
|
@ -68,6 +87,7 @@ UDPWiimote::UDPWiimote(const char *port) : d(new _d) ,x(0),y(0),z(0),nunX(0),nun
|
|||
#endif
|
||||
|
||||
noinst++;
|
||||
// NOTICE_LOG(WIIMOTE,"UDPWii instantiated");
|
||||
|
||||
memset(&hints, 0, sizeof hints);
|
||||
hints.ai_family = AF_INET;
|
||||
|
|
@ -75,148 +95,206 @@ UDPWiimote::UDPWiimote(const char *port) : d(new _d) ,x(0),y(0),z(0),nunX(0),nun
|
|||
hints.ai_flags = AI_PASSIVE; // use my IP
|
||||
|
||||
if ((rv = getaddrinfo(NULL, port, &hints, &servinfo)) != 0) {
|
||||
// fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
|
||||
cleanup;
|
||||
err=-1;
|
||||
return;
|
||||
}
|
||||
|
||||
// loop through all the results and bind to the first we can
|
||||
|
||||
// loop through all the results and bind to everything we can
|
||||
for(p = servinfo; p != NULL; p = p->ai_next) {
|
||||
if (((d->sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)),d->sockfd) == BAD_SOCK) {
|
||||
sock_t sock;
|
||||
if ((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == BAD_SOCK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (bind(d->sockfd, p->ai_addr, p->ai_addrlen) == -1) {
|
||||
close(d->sockfd);
|
||||
if (bind(sock, p->ai_addr, p->ai_addrlen) == -1) {
|
||||
close(sock);
|
||||
continue;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
//NOTICE_LOG(WIIMOTE,"UDPWii new listening sock");
|
||||
d->sockfds.push_back(sock);
|
||||
}
|
||||
|
||||
if (p == NULL) {
|
||||
if (d->sockfds.empty()) {
|
||||
cleanup;
|
||||
err=-2;
|
||||
return;
|
||||
}
|
||||
|
||||
freeaddrinfo(servinfo);
|
||||
blockingoff(d->sockfd);
|
||||
err=0;
|
||||
d->exit=false;
|
||||
// NOTICE_LOG(WIIMOTE,"UDPWii thread starting");
|
||||
d->termLock.Enter();
|
||||
d->thread = new Common::Thread(UDPWiiThread,this);
|
||||
d->termLock.Leave();
|
||||
return;
|
||||
}
|
||||
|
||||
void UDPWiimote::mainThread()
|
||||
{
|
||||
d->termLock.Enter();
|
||||
// NOTICE_LOG(WIIMOTE,"UDPWii thread started");
|
||||
fd_set fds;
|
||||
struct timeval timeout;
|
||||
timeout.tv_sec=1;
|
||||
timeout.tv_usec=500000;
|
||||
//Common::Thread * thisthr= d->thread;
|
||||
do
|
||||
{
|
||||
int maxfd=0;
|
||||
FD_ZERO(&fds);
|
||||
for (std::list<sock_t>::iterator i=d->sockfds.begin(); i!=d->sockfds.end(); i++)
|
||||
{
|
||||
FD_SET(*i,&fds);
|
||||
#ifndef _WIN32
|
||||
if (*i>=maxfd)
|
||||
maxfd=(*i)+1;
|
||||
#endif
|
||||
}
|
||||
d->termLock.Leave();
|
||||
if (d->exit) return;
|
||||
int rt=select(maxfd,&fds,NULL,NULL,&timeout);
|
||||
if (d->exit) return;
|
||||
d->termLock.Enter();
|
||||
if (d->exit) return;
|
||||
if (rt)
|
||||
{
|
||||
for (std::list<sock_t>::iterator i=d->sockfds.begin(); i!=d->sockfds.end(); i++)
|
||||
if (FD_ISSET(*i,&fds))
|
||||
{
|
||||
sock_t fd=*i;
|
||||
u8 bf[64];
|
||||
int size=60;
|
||||
size_t addr_len;
|
||||
struct sockaddr_storage their_addr;
|
||||
addr_len = sizeof their_addr;
|
||||
if ((size = recvfrom(fd, (dataz)bf, size , 0,(struct sockaddr *)&their_addr, (socklen_t*)&addr_len)) == -1)
|
||||
{
|
||||
ERROR_LOG(WIIMOTE,"UDPWii Packet error");
|
||||
}
|
||||
else
|
||||
{
|
||||
d->mutex.Enter();
|
||||
if (pharsePacket(bf,size)==0)
|
||||
{
|
||||
//NOTICE_LOG(WIIMOTE,"UDPWII New pack");
|
||||
} else {
|
||||
//NOTICE_LOG(WIIMOTE,"UDPWII Wrong pack format... ignoring");
|
||||
}
|
||||
d->mutex.Leave();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
broadcastPresence();
|
||||
}
|
||||
} while (!(d->exit));
|
||||
d->termLock.Leave();
|
||||
//delete thisthr;
|
||||
}
|
||||
|
||||
UDPWiimote::~UDPWiimote()
|
||||
{
|
||||
close(d->sockfd);
|
||||
d->exit=true;
|
||||
d->termLock.Enter();
|
||||
d->termLock.Leave();
|
||||
for (std::list<sock_t>::iterator i=d->sockfds.begin(); i!=d->sockfds.end(); i++)
|
||||
close(*i);
|
||||
cleanup;
|
||||
delete d;
|
||||
}
|
||||
|
||||
int UDPWiimote::readPack(void * data, int *size)
|
||||
{
|
||||
int numbytes;
|
||||
size_t addr_len;
|
||||
struct sockaddr_storage their_addr;
|
||||
addr_len = sizeof their_addr;
|
||||
if ((numbytes = recvfrom(d->sockfd, (dataz)data, (*size) , 0,
|
||||
(struct sockaddr *)&their_addr, (socklen_t*)&addr_len)) == -1) {
|
||||
if (ERRNO==EWOULDBLOCK)
|
||||
return -1;
|
||||
return -2;
|
||||
} else
|
||||
(*size)=numbytes;
|
||||
return 0;
|
||||
}
|
||||
|
||||
#define ACCEL_FLAG (1<<0)
|
||||
#define BUTT_FLAG (1<<1)
|
||||
#define IR_FLAG (1<<2)
|
||||
#define NUN_FLAG (1<<3)
|
||||
|
||||
void UDPWiimote::update()
|
||||
int UDPWiimote::pharsePacket(u8 * bf, size_t size)
|
||||
{
|
||||
u8 bf[64];
|
||||
int size=60;
|
||||
int res=0;
|
||||
u8 time=0;
|
||||
int nopack=0;
|
||||
for (int i=0; (res=readPack(&bf,&size)),(i<100)&&(res!=-1); (res<-1)?i++:0)
|
||||
if (size<3) return -1;
|
||||
if (bf[0]!=0xde)
|
||||
return -1;
|
||||
if (bf[1]==0)
|
||||
time=0;
|
||||
if (bf[1]<time)
|
||||
return -1;
|
||||
time=bf[1];
|
||||
u32 *p=(u32*)(&bf[3]);
|
||||
if (bf[2]&ACCEL_FLAG)
|
||||
{
|
||||
if (res==0)
|
||||
{
|
||||
if (bf[0]==0xde)
|
||||
{
|
||||
if (bf[1]==0)
|
||||
time=0;
|
||||
if (bf[1]>=time) //packet timestamp. assures order is maintained
|
||||
{
|
||||
nopack++;
|
||||
time=bf[1];
|
||||
u32 *p=(u32*)(&bf[3]);
|
||||
if (bf[2]&ACCEL_FLAG)
|
||||
{
|
||||
double ux,uy,uz;
|
||||
ux=(double)((s32)ntohl(*p)); p++;
|
||||
uy=(double)((s32)ntohl(*p)); p++;
|
||||
uz=(double)((s32)ntohl(*p)); p++;
|
||||
x=ux/1048576; //packet accel data
|
||||
y=uy/1048576;
|
||||
z=uz/1048576;
|
||||
}
|
||||
if (bf[2]&BUTT_FLAG)
|
||||
{
|
||||
mask=ntohl(*p); p++;
|
||||
}
|
||||
if (bf[2]&IR_FLAG)
|
||||
{
|
||||
pointerX=((double)((s32)ntohl(*p)))/1048576; p++;
|
||||
pointerY=((double)((s32)ntohl(*p)))/1048576; p++;
|
||||
}
|
||||
if (bf[2]&NUN_FLAG)
|
||||
{
|
||||
nunMask=*((u8*)p); p=(u32*)(((u8*)p)+1);
|
||||
nunX=((double)((s32)ntohl(*p)))/1048576; p++;
|
||||
nunY=((double)((s32)ntohl(*p)))/1048576; p++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (res==-2)
|
||||
{
|
||||
ERROR_LOG(WIIMOTE,"UDPWii Packet error");
|
||||
}
|
||||
if ((size-(((u8*)p)-bf))<12) return -1;
|
||||
double ux,uy,uz;
|
||||
ux=(double)((s32)ntohl(*p)); p++;
|
||||
uy=(double)((s32)ntohl(*p)); p++;
|
||||
uz=(double)((s32)ntohl(*p)); p++;
|
||||
x=ux/1048576; //packet accel data
|
||||
y=uy/1048576;
|
||||
z=uz/1048576;
|
||||
}
|
||||
//NOTICE_LOG(WIIMOTE,"UDPWii update result:np:%d x:%f y:%f z:%f nx:%f ny:%f px:%f py:%f bmask:%x nmask:%x",
|
||||
// nopack, x, y, z, nunX, nunY, pointerX, pointerY, mask, nunMask);
|
||||
if (bf[2]&BUTT_FLAG)
|
||||
{
|
||||
if ((size-(((u8*)p)-bf))<4) return -1;
|
||||
mask=ntohl(*p); p++;
|
||||
}
|
||||
if (bf[2]&IR_FLAG)
|
||||
{
|
||||
if ((size-(((u8*)p)-bf))<8) return -1;
|
||||
pointerX=((double)((s32)ntohl(*p)))/1048576; p++;
|
||||
pointerY=((double)((s32)ntohl(*p)))/1048576; p++;
|
||||
}
|
||||
if (bf[2]&NUN_FLAG)
|
||||
{
|
||||
if ((size-(((u8*)p)-bf))<9) return -1;
|
||||
nunMask=*((u8*)p); p=(u32*)(((u8*)p)+1);
|
||||
nunX=((double)((s32)ntohl(*p)))/1048576; p++;
|
||||
nunY=((double)((s32)ntohl(*p)))/1048576; p++;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void UDPWiimote::broadcastPresence()
|
||||
{
|
||||
// NOTICE_LOG(WIIMOTE,"UDPWii broadcasting presence");
|
||||
}
|
||||
|
||||
void UDPWiimote::getAccel(int &_x, int &_y, int &_z)
|
||||
{
|
||||
//NOTICE_LOG(WIIMOTE,"%lf %lf %lf",_x, _y, _z);
|
||||
float xg = WiiMoteEmu::g_wm.cal_g.x;
|
||||
float yg = WiiMoteEmu::g_wm.cal_g.y;
|
||||
float zg = WiiMoteEmu::g_wm.cal_g.z;
|
||||
d->mutex.Enter();
|
||||
_x = WiiMoteEmu::g_wm.cal_zero.x + (int)(xg * x);
|
||||
_y = WiiMoteEmu::g_wm.cal_zero.y + (int)(yg * y);
|
||||
_z = WiiMoteEmu::g_wm.cal_zero.z + (int)(zg * z);
|
||||
d->mutex.Leave();
|
||||
//NOTICE_LOG(WIIMOTE,"%lf %lf %lf",_x, _y, _z);
|
||||
}
|
||||
|
||||
u32 UDPWiimote::getButtons()
|
||||
{
|
||||
return mask;
|
||||
u32 msk;
|
||||
d->mutex.Enter();
|
||||
msk=mask;
|
||||
d->mutex.Leave();
|
||||
return msk;
|
||||
}
|
||||
|
||||
void UDPWiimote::getIR(float &_x, float &_y)
|
||||
{
|
||||
d->mutex.Enter();
|
||||
_x=(float)pointerX;
|
||||
_y=(float)pointerY;
|
||||
d->mutex.Leave();
|
||||
}
|
||||
|
||||
void UDPWiimote::getNunchuck(float &_x, float &_y, u8 &_mask)
|
||||
{
|
||||
d->mutex.Enter();
|
||||
_x=(float)nunX;
|
||||
_y=(float)nunY;
|
||||
_mask=nunMask;
|
||||
d->mutex.Leave();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,15 +23,15 @@ class UDPWiimote
|
|||
public:
|
||||
UDPWiimote(const char * port);
|
||||
virtual ~UDPWiimote();
|
||||
void update();
|
||||
void getAccel(int &x, int &y, int &z);
|
||||
u32 getButtons();
|
||||
void getNunchuck(float &x, float &y, u8 &mask);
|
||||
void getIR(float &x, float &y);
|
||||
int getErrNo() {return err;};
|
||||
private:
|
||||
int readPack(void * data, int *size);
|
||||
struct _d; //using pimpl because SOCKET on windows is defined in Winsock2.h, witch doesen't have include guards
|
||||
int pharsePacket(u8 * data, size_t size);
|
||||
void mainThread();
|
||||
struct _d; //using pimpl because Winsock2.h doesen't have include guards -_-
|
||||
_d *d;
|
||||
double x,y,z;
|
||||
double nunX,nunY;
|
||||
|
|
@ -40,5 +40,8 @@ private:
|
|||
u32 mask;
|
||||
int err;
|
||||
static int noinst;
|
||||
friend void _UDPWiiThread(void* arg);
|
||||
void broadcastPresence();
|
||||
u8 time;
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue