A multi-interface listener, threaded approach to UDPWii

git-svn-id: https://dolphin-emu.googlecode.com/svn/trunk@5821 8ced0084-cf51-0410-be5f-012b33b47a6e

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();
 

Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.cpp

@@ -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) {
+        if (bind(sock, p->ai_addr, p->ai_addrlen) == -1) {
-            close(d->sockfd);
+            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];
+	if (size<3) return -1;
-	int size=60;
+	if (bf[0]!=0xde)
-	int res=0;
+		return -1;
-	u8 time=0;
+	if (bf[1]==0)
-	int nopack=0;
+		time=0;
-	for (int i=0; (res=readPack(&bf,&size)),(i<100)&&(res!=-1); (res<-1)?i++:0)
+	if (bf[1]<time)
+		return -1;
+	time=bf[1];
+	u32 *p=(u32*)(&bf[3]);
+	if (bf[2]&ACCEL_FLAG)
 	{
-		if (res==0)
+		if ((size-(((u8*)p)-bf))<12) return -1;
-		{
+		double ux,uy,uz;
-			if (bf[0]==0xde)
+		ux=(double)((s32)ntohl(*p)); p++;
-			{
+		uy=(double)((s32)ntohl(*p)); p++;
-				if (bf[1]==0)
+		uz=(double)((s32)ntohl(*p)); p++;
-					time=0;
+		x=ux/1048576; //packet accel data
-				if (bf[1]>=time) //packet timestamp. assures order is maintained
+		y=uy/1048576;
-				{
+		z=uz/1048576;
-					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");
-		} 
 	}
-	//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",
+	if (bf[2]&BUTT_FLAG)
-	//	nopack, x, y, z, nunX, nunY, pointerX, pointerY, mask, nunMask);
+	{
+		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();
 }

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);
+	int pharsePacket(u8 * data, size_t size);
-	struct _d; //using pimpl because SOCKET on windows is defined in Winsock2.h, witch doesen't have include guards
+	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