Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.cpp

   1 #include "UDPWiimote.h"
   2
   3 #ifdef _WIN32
   4
   5 #include <winsock2.h>
   6 #include <ws2tcpip.h>
   7
   8 #define sock_t SOCKET
   9 #define ERRNO WSAGetLastError()
  10 #define EWOULDBLOCK WSAEWOULDBLOCK
  11 #define BAD_SOCK INVALID_SOCKET
  12 #define close(x) closesocket(x)
  13 #define cleanup do {noinst--; if (noinst==0) WSACleanup();} while (0)
  14 #define blockingoff(sock) ioctlsocket(sock, FIONBIO, &iMode)
  15 #define dataz char*
  16 #ifdef _MSC_VER
  17 #pragma comment (lib, "Ws2_32.lib")
  18 #endif
  19
  20 #else
  21
  22 #include <stdlib.h>
  23 #include <unistd.h>
  24 #include <errno.h>
  25 #include <sys/types.h>
  26 #include <sys/socket.h>
  27 #include <netinet/in.h>
  28 #include <arpa/inet.h>
  29 #include <netdb.h>
  30 #include <unistd.h>
  31 #include <fcntl.h>
  32 #define BAD_SOCK -1
  33 #define ERRNO errno
  34 #define cleanup noinst--
  35 #define blockingoff(sock) fcntl(sock, F_SETFL, O_NONBLOCK)
  36 #define dataz void*
  37 #define sock_t int
  38
  39 #endif
  40
  41 #include "EmuMain.h"
  42 #include <stdio.h>
  43 #include <string.h>
  44
  45 struct UDPWiimote::_d
  46 {
  47         sock_t sockfd;
  48 };
  49
  50 int UDPWiimote::noinst=0;
  51
  52 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)
  53 {
  54         #ifdef _WIN32
  55         u_long iMode = 1;
  56         #endif
  57     struct addrinfo hints, *servinfo, *p;
  58     int rv;
  59
  60         #ifdef _WIN32
  61         if (noinst==0)
  62         {
  63                 WORD sockVersion;
  64                 WSADATA wsaData;
  65                 sockVersion = MAKEWORD(2, 2);
  66                 WSAStartup(sockVersion, &wsaData);
  67         }
  68         #endif
  69
  70         noinst++;
  71
  72     memset(&hints, 0, sizeof hints);
  73         hints.ai_family = AF_INET;
  74     hints.ai_socktype = SOCK_DGRAM;
  75     hints.ai_flags = AI_PASSIVE; // use my IP
  76
  77     if ((rv = getaddrinfo(NULL, port, &hints, &servinfo)) != 0) {
  78 //        fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
  79                 cleanup;
  80         err=-1;
  81                 return;
  82     }
  83
  84     // loop through all the results and bind to the first we can
  85     for(p = servinfo; p != NULL; p = p->ai_next) {
  86         if (((d->sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)),d->sockfd) == BAD_SOCK) {
  87                         continue;
  88         }
  89
  90         if (bind(d->sockfd, p->ai_addr, p->ai_addrlen) == -1) {
  91             close(d->sockfd);
  92                         continue;
  93         }
  94
  95         break;
  96     }
  97
  98     if (p == NULL) {
  99                 cleanup;
 100                 err=-2;
 101         return;
 102     }
 103
 104     freeaddrinfo(servinfo);
 105         blockingoff(d->sockfd);
 106         err=0;
 107         return;
 108 }
 109
 110 UDPWiimote::~UDPWiimote()
 111 {
 112         close(d->sockfd);
 113         cleanup;
 114         delete d;
 115 }
 116
 117 int UDPWiimote::readPack(void * data, int *size)
 118 {
 119     int numbytes;
 120     size_t addr_len;
 121     struct sockaddr_storage their_addr;
 122         addr_len = sizeof their_addr;
 123     if ((numbytes = recvfrom(d->sockfd, (dataz)data, (*size) , 0,
 124                                                          (struct sockaddr *)&their_addr, (socklen_t*)&addr_len)) == -1) {
 125                 if (ERRNO==EWOULDBLOCK)
 126                         return -1;
 127         return -2;
 128     } else
 129                 (*size)=numbytes;
 130         return 0;
 131 }
 132
 133 #define ACCEL_FLAG (1<<0)
 134 #define BUTT_FLAG (1<<1)
 135 #define IR_FLAG (1<<2)
 136 #define NUN_FLAG (1<<3)
 137
 138 void UDPWiimote::update()
 139 {
 140         u8 bf[64];
 141         int size=60;
 142         int res=0;
 143         u8 time=0;
 144         int nopack=0;
 145         for (int i=0; (res=readPack(&bf,&size)),(i<100)&&(res!=-1); (res<-1)?i++:0)
 146         {
 147                 if (res==0)
 148                 {
 149                         if (bf[0]==0xde)
 150                         {
 151                                 if (bf[1]==0)
 152                                         time=0;
 153                                 if (bf[1]>=time) //packet timestamp. assures order is maintained
 154                                 {
 155                                         nopack++;
 156                                         time=bf[1];
 157                                         u32 *p=(u32*)(&bf[3]);
 158                                         if (bf[2]&ACCEL_FLAG)
 159                                         {
 160                                                 double ux,uy,uz;
 161                                                 ux=(double)((s32)ntohl(*p)); p++;
 162                                                 uy=(double)((s32)ntohl(*p)); p++;
 163                                                 uz=(double)((s32)ntohl(*p)); p++;
 164                                                 x=ux/1048576; //packet accel data
 165                                                 y=uy/1048576;
 166                                                 z=uz/1048576;
 167                                         }
 168                                         if (bf[2]&BUTT_FLAG)
 169                                         {
 170                                                 mask=ntohl(*p); p++;
 171                                         }
 172                                         if (bf[2]&IR_FLAG)
 173                                         {
 174                                                 pointerX=((double)((s32)ntohl(*p)))/1048576; p++;
 175                                                 pointerY=((double)((s32)ntohl(*p)))/1048576; p++;
 176                                         }
 177                                         if (bf[2]&NUN_FLAG)
 178                                         {
 179                                                 nunMask=*((u8*)p); p=(u32*)(((u8*)p)+1);
 180                                                 nunX=((double)((s32)ntohl(*p)))/1048576; p++;
 181                                                 nunY=((double)((s32)ntohl(*p)))/1048576; p++;
 182                                         }
 183                                 }
 184                         }
 185                 }
 186                 if (res==-2)
 187                 {
 188                         ERROR_LOG(WIIMOTE,"UDPWii Packet error");
 189                 }
 190         }
 191         //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",
 192         //      nopack, x, y, z, nunX, nunY, pointerX, pointerY, mask, nunMask);
 193 }
 194
 195 void UDPWiimote::getAccel(int &_x, int &_y, int &_z)
 196 {
 197         //NOTICE_LOG(WIIMOTE,"%lf %lf %lf",_x, _y, _z);
 198         float xg = WiiMoteEmu::g_wm.cal_g.x;
 199         float yg = WiiMoteEmu::g_wm.cal_g.y;
 200         float zg = WiiMoteEmu::g_wm.cal_g.z;
 201         _x = WiiMoteEmu::g_wm.cal_zero.x + (int)(xg * x);
 202         _y = WiiMoteEmu::g_wm.cal_zero.y + (int)(yg * y);
 203         _z = WiiMoteEmu::g_wm.cal_zero.z + (int)(zg * z);
 204 }
 205
 206 u32 UDPWiimote::getButtons()
 207 {
 208         return mask;
 209 }
 210
 211 void UDPWiimote::getIR(float &_x, float &_y)
 212 {
 213         _x=(float)pointerX;
 214         _y=(float)pointerY;
 215 }
 216
 217 void UDPWiimote::getNunchuck(float &_x, float &_y, u8 &_mask)
 218 {
 219         _x=(float)nunX;
 220         _y=(float)nunY;
 221         _mask=nunMask;
 222 }