Source/Core/InputCommon/Src/UDPWiimote.cpp

   1 #include "UDPWiimote.h"
   2
   3 #ifdef _WIN32
   4
   5 #include <winsock2.h>
   6 #include <ws2tcpip.h>
   7 #define sock_t SOCKET
   8 #define ERRNO WSAGetLastError()
   9 #define EWOULDBLOCK WSAEWOULDBLOCK
  10 #define BAD_SOCK INVALID_SOCKET
  11 #define close(x) closesocket(x)
  12 #define cleanup do {noinst--; if (noinst==0) WSACleanup();} while (0)
  13 #define blockingoff(sock) ioctlsocket(sock, FIONBIO, &iMode)
  14 #define dataz char*
  15 #ifdef _MSC_VER
  16 #pragma comment (lib, "Ws2_32.lib")
  17 #endif
  18
  19 #else
  20
  21 #include <stdlib.h>
  22 #include <unistd.h>
  23 #include <errno.h>
  24 #include <sys/types.h>
  25 #include <sys/socket.h>
  26 #include <netinet/in.h>
  27 #include <arpa/inet.h>
  28 #include <netdb.h>
  29 #include <unistd.h>
  30 #include <fcntl.h>
  31 #define BAD_SOCK -1
  32 #define ERRNO errno
  33 #define cleanup noinst--
  34 #define blockingoff(sock) fcntl(sock, F_SETFL, O_NONBLOCK)
  35 #define dataz void*
  36 #define sock_t int
  37
  38 #endif
  39
  40 #include "Thread.h"
  41 #include <stdio.h>
  42 #include <string.h>
  43 #include <list>
  44
  45 struct UDPWiimote::_d
  46 {
  47         Common::Thread * thread;
  48         std::list<sock_t> sockfds;
  49         Common::CriticalSection termLock,mutex;
  50         volatile bool exit;
  51 };
  52
  53 int UDPWiimote::noinst=0;
  54
  55 void _UDPWiiThread(void* arg)
  56 {
  57         ((UDPWiimote*)arg)->mainThread();
  58         //NOTICE_LOG(WIIMOTE,"UDPWii thread stopped");
  59 }
  60
  61 THREAD_RETURN UDPWiiThread(void* arg)
  62 {
  63         _UDPWiiThread(arg);
  64         return 0;
  65 }
  66
  67 UDPWiimote::UDPWiimote(const char *_port) :
  68         port(_port),
  69         d(new _d) ,x(0),y(0),z(0),nunX(0),nunY(0),
  70         pointerX(-0.1),pointerY(-0.1),nunMask(0),mask(0),time(0)
  71 {
  72         #ifdef _WIN32
  73         u_long iMode = 1;
  74         #endif
  75         struct addrinfo hints, *servinfo, *p;
  76         int rv;
  77         d->thread=NULL;
  78
  79         #ifdef _WIN32
  80         if (noinst==0)
  81         {
  82                 WORD sockVersion;
  83                 WSADATA wsaData;
  84                 sockVersion = MAKEWORD(2, 2);
  85                 WSAStartup(sockVersion, &wsaData);
  86         }
  87         #endif
  88
  89         noinst++;
  90         //PanicAlert("UDPWii instantiated");
  91
  92         memset(&hints, 0, sizeof hints);
  93         hints.ai_family = AF_INET;
  94         hints.ai_socktype = SOCK_DGRAM;
  95         hints.ai_flags = AI_PASSIVE; // use my IP
  96
  97         if ((rv = getaddrinfo(NULL, _port, &hints, &servinfo)) != 0) {
  98                 cleanup;
  99                 err=-1;
 100                 return;
 101         }
 102
 103         // loop through all the results and bind to everything we can
 104         for(p = servinfo; p != NULL; p = p->ai_next) {
 105                 sock_t sock;
 106                 if ((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == BAD_SOCK) {
 107                         continue;
 108                 }
 109
 110                 if (bind(sock, p->ai_addr, p->ai_addrlen) == -1) {
 111                         close(sock);
 112                         continue;
 113                 }
 114
 115
 116                 //NOTICE_LOG(WIIMOTE,"UDPWii new listening sock");
 117                 d->sockfds.push_back(sock);
 118         }
 119
 120         if (d->sockfds.empty()) {
 121                 cleanup;
 122                 err=-2;
 123                 return;
 124         }
 125
 126         freeaddrinfo(servinfo);
 127         err=0;
 128         d->exit=false;
 129 //      NOTICE_LOG(WIIMOTE,"UDPWii thread starting");
 130         d->termLock.Enter();
 131         d->thread = new Common::Thread(UDPWiiThread,this);
 132         d->termLock.Leave();
 133         return;
 134 }
 135
 136 void UDPWiimote::mainThread()
 137 {
 138         d->termLock.Enter();
 139 //      NOTICE_LOG(WIIMOTE,"UDPWii thread started");
 140         fd_set fds;
 141         struct timeval timeout;
 142         timeout.tv_sec=1;
 143         timeout.tv_usec=500000;
 144         //Common::Thread * thisthr= d->thread;
 145         do
 146         {
 147                 int maxfd=0;
 148                 FD_ZERO(&fds);
 149                 for (std::list<sock_t>::iterator i=d->sockfds.begin(); i!=d->sockfds.end(); i++)
 150                 {
 151                         FD_SET(*i,&fds);
 152 #ifndef _WIN32
 153                         if (*i>=maxfd)
 154                                 maxfd=(*i)+1;
 155 #endif
 156                 }
 157                 d->termLock.Leave();
 158                 if (d->exit) return;
 159                 int rt=select(maxfd,&fds,NULL,NULL,&timeout);
 160                 if (d->exit) return;
 161                 d->termLock.Enter();
 162                 if (d->exit) return;
 163                 if (rt)
 164                 {
 165                         for (std::list<sock_t>::iterator i=d->sockfds.begin(); i!=d->sockfds.end(); i++)
 166                                 if (FD_ISSET(*i,&fds))
 167                                 {
 168                                         sock_t fd=*i;
 169                                         u8 bf[64];
 170                                         int size=60;
 171                                         size_t addr_len;
 172                                         struct sockaddr_storage their_addr;
 173                                         addr_len = sizeof their_addr;
 174                                         if ((size = recvfrom(fd, (dataz)bf, size , 0,(struct sockaddr *)&their_addr, (socklen_t*)&addr_len)) == -1)
 175                                         {
 176                                                 ERROR_LOG(WIIMOTE,"UDPWii Packet error");
 177                                         }
 178                                         else
 179                                         {
 180                                                 d->mutex.Enter();
 181                                                 if (pharsePacket(bf,size)==0)
 182                                                 {
 183                                                         //NOTICE_LOG(WIIMOTE,"UDPWII New pack");
 184                                                 } else {
 185                                                         //NOTICE_LOG(WIIMOTE,"UDPWII Wrong pack format... ignoring");
 186                                                 }
 187                                                 d->mutex.Leave();
 188                                         }
 189                                 }
 190                 } else {
 191                         broadcastPresence();
 192                 }
 193         } while (!(d->exit));
 194         d->termLock.Leave();
 195         //delete thisthr;
 196 }
 197
 198 UDPWiimote::~UDPWiimote()
 199 {
 200         d->exit=true;
 201         d->thread->WaitForDeath();
 202         d->termLock.Enter();
 203         d->termLock.Leave();
 204         for (std::list<sock_t>::iterator i=d->sockfds.begin(); i!=d->sockfds.end(); i++)
 205                 close(*i);
 206         cleanup;
 207         delete d;
 208         //PanicAlert("UDPWii destructed");
 209 }
 210
 211 #define ACCEL_FLAG (1<<0)
 212 #define BUTT_FLAG (1<<1)
 213 #define IR_FLAG (1<<2)
 214 #define NUN_FLAG (1<<3)
 215
 216 int UDPWiimote::pharsePacket(u8 * bf, size_t size)
 217 {
 218         if (size<3) return -1;
 219         if (bf[0]!=0xde)
 220                 return -1;
 221         if (bf[1]==0)
 222                 time=0;
 223         if (bf[1]<time)
 224                 return -1;
 225         time=bf[1];
 226         u32 *p=(u32*)(&bf[3]);
 227         if (bf[2]&ACCEL_FLAG)
 228         {
 229                 if ((size-(((u8*)p)-bf))<12) return -1;
 230                 double ux,uy,uz;
 231                 ux=(double)((s32)ntohl(*p)); p++;
 232                 uy=(double)((s32)ntohl(*p)); p++;
 233                 uz=(double)((s32)ntohl(*p)); p++;
 234                 x=ux/1048576; //packet accel data
 235                 y=uy/1048576;
 236                 z=uz/1048576;
 237         }
 238         if (bf[2]&BUTT_FLAG)
 239         {
 240                 if ((size-(((u8*)p)-bf))<4) return -1;
 241                 mask=ntohl(*p); p++;
 242         }
 243         if (bf[2]&IR_FLAG)
 244         {
 245                 if ((size-(((u8*)p)-bf))<8) return -1;
 246                 pointerX=((double)((s32)ntohl(*p)))/1048576; p++;
 247                 pointerY=((double)((s32)ntohl(*p)))/1048576; p++;
 248         }
 249         if (bf[2]&NUN_FLAG)
 250         {
 251                 if ((size-(((u8*)p)-bf))<9) return -1;
 252                 nunMask=*((u8*)p); p=(u32*)(((u8*)p)+1);
 253                 nunX=((double)((s32)ntohl(*p)))/1048576; p++;
 254                 nunY=((double)((s32)ntohl(*p)))/1048576; p++;
 255         }
 256         return 0;
 257 }
 258
 259
 260 void UDPWiimote::broadcastPresence()
 261 {
 262 //      NOTICE_LOG(WIIMOTE,"UDPWii broadcasting presence");
 263 }
 264
 265 void UDPWiimote::getAccel(float &_x, float &_y, float &_z)
 266 {
 267         d->mutex.Enter();
 268         _x=x;
 269         _y=y;
 270         _z=z;
 271         d->mutex.Leave();
 272         //NOTICE_LOG(WIIMOTE,"%lf %lf %lf",_x, _y, _z);
 273 }
 274
 275 u32 UDPWiimote::getButtons()
 276 {
 277         u32 msk;
 278         d->mutex.Enter();
 279         msk=mask;
 280         d->mutex.Leave();
 281         return msk;
 282 }
 283
 284 void UDPWiimote::getIR(float &_x, float &_y)
 285 {
 286         d->mutex.Enter();
 287         _x=(float)pointerX;
 288         _y=(float)pointerY;
 289         d->mutex.Leave();
 290 }
 291
 292 void UDPWiimote::getNunchuck(float &_x, float &_y, u8 &_mask)
 293 {
 294         d->mutex.Enter();
 295         _x=(float)nunX;
 296         _y=(float)nunY;
 297         _mask=nunMask;
 298         d->mutex.Leave();
 299 }
 300
 301 const char * UDPWiimote::getPort()
 302 {
 303         return port.c_str();
 304 }