dolphin/Source/Plugins/Plugin_Wiimote/Src/UDPWiimote.cpp

222 lines
5.0 KiB
C++
Raw Normal View History

#include "UDPWiimote.h"
#ifdef _WIN32
#include <winsock2.h>
#include <ws2tcpip.h>
#define sock_t SOCKET
#define ERRNO WSAGetLastError()
#define EWOULDBLOCK WSAEWOULDBLOCK
#define BAD_SOCK INVALID_SOCKET
#define close(x) closesocket(x)
#define cleanup do {noinst--; if (noinst==0) WSACleanup();} while (0)
#define blockingoff(sock) ioctlsocket(sock, FIONBIO, &iMode)
#define dataz char*
#ifdef _MSC_VER
#pragma comment (lib, "Ws2_32.lib")
#endif
#else
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <unistd.h>
#include <fcntl.h>
#define BAD_SOCK -1
#define ERRNO errno
#define cleanup noinst--
#define blockingoff(sock) fcntl(sock, F_SETFL, O_NONBLOCK)
#define dataz void*
#define sock_t int
#endif
#include "EmuMain.h"
#include <stdio.h>
#include <string.h>
struct UDPWiimote::_d
{
sock_t sockfd;
};
int UDPWiimote::noinst=0;
UDPWiimote::UDPWiimote(const char *port) : d(new _d) ,x(0),y(0),z(0),nunX(0),nunY(0),mask(0),nunMask(0),pointerX(-0.1),pointerY(-0.1)
{
#ifdef _WIN32
u_long iMode = 1;
#endif
struct addrinfo hints, *servinfo, *p;
int rv;
#ifdef _WIN32
if (noinst==0)
{
WORD sockVersion;
WSADATA wsaData;
sockVersion = MAKEWORD(2, 2);
WSAStartup(sockVersion, &wsaData);
}
#endif
noinst++;
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_DGRAM;
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;
this->err=-1;
return;
}
// loop through all the results and bind to the first 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) {
continue;
}
if (bind(d->sockfd, p->ai_addr, p->ai_addrlen) == -1) {
close(d->sockfd);
continue;
}
break;
}
if (p == NULL) {
cleanup;
this->err=-2;
return;
}
freeaddrinfo(servinfo);
blockingoff(d->sockfd);
this->err=0;
return;
}
UDPWiimote::~UDPWiimote()
{
close(d->sockfd);
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()
{
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 (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++;
this->x=ux/1048576; //packet accel data
this->y=uy/1048576;
this->z=uz/1048576;
}
if (bf[2]&BUTT_FLAG)
{
mask=ntohl(*p); p++;
}
if (bf[2]&IR_FLAG)
{
this->pointerX=((double)((s32)ntohl(*p)))/1048576; p++;
this->pointerY=((double)((s32)ntohl(*p)))/1048576; p++;
}
if (bf[2]&NUN_FLAG)
{
this->nunMask=*((u8*)p); p=(u32*)(((u8*)p)+1);
this->nunX=((double)((s32)ntohl(*p)))/1048576; p++;
this->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",
// nopack,this->x,this->y,this->z,this->nunX, this->nunY,this->pointerX,this->pointerY, this->mask, this->nunMask);
}
void UDPWiimote::getAccel(int &x, int &y, int &z)
{
//NOTICE_LOG(WIIMOTE,"%lf %lf %lf",this->x, this-y, this->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;
x = WiiMoteEmu::g_wm.cal_zero.x + (int)(xg * this->x);
y = WiiMoteEmu::g_wm.cal_zero.y + (int)(yg * this->y);
z = WiiMoteEmu::g_wm.cal_zero.z + (int)(zg * this->z);
}
u32 UDPWiimote::getButtons()
{
return mask;
}
void UDPWiimote::getIR(float &x, float &y)
{
x=(float)this->pointerX;
y=(float)this->pointerY;
}
void UDPWiimote::getNunchuck(float &x, float &y, u8 &mask)
{
x=(float)this->nunX;
y=(float)this->nunY;
mask=this->nunMask;
}