From 0dc8833396ab26377b4cd1ba952371e74b94e739 Mon Sep 17 00:00:00 2001 From: dapetcu21 Date: Sat, 3 Jul 2010 04:56:37 +0000 Subject: [PATCH] A multi-interface listener, threaded approach to UDPWii git-svn-id: https://dolphin-emu.googlecode.com/svn/trunk@5821 8ced0084-cf51-0410-be5f-012b33b47a6e --- .../Plugins/Plugin_Wiimote/Src/FillReport.cpp | 4 +- .../Plugins/Plugin_Wiimote/Src/UDPWiimote.cpp | 244 ++++++++++++------ .../Plugins/Plugin_Wiimote/Src/UDPWiimote.h | 9 +- 3 files changed, 168 insertions(+), 89 deletions(-) diff --git a/Source/Plugins/Plugin_Wiimote/Src/FillReport.cpp b/Source/Plugins/Plugin_Wiimote/Src/FillReport.cpp index b9f50291b5..66c1a214b4 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/FillReport.cpp +++ b/Source/Plugins/Plugin_Wiimote/Src/FillReport.cpp @@ -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(); diff --git a/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.cpp b/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.cpp index 37e0b7521e..54b0cb055d 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.cpp +++ b/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.cpp @@ -4,7 +4,6 @@ #include #include - #define sock_t SOCKET #define ERRNO WSAGetLastError() #define EWOULDBLOCK WSAEWOULDBLOCK @@ -38,24 +37,44 @@ #endif +#include "Thread.h" #include "EmuMain.h" #include #include +#include struct UDPWiimote::_d { - sock_t sockfd; + Common::Thread * thread; + std::list 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::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::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::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) //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(); } diff --git a/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.h b/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.h index 0c26d56ab9..8e4e2d28fc 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.h +++ b/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.h @@ -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