made bluetooth connection work

5 files changed, 647 insertions, 352 deletions

diff --git a/Makefile b/Makefile
index f6457e3..d1a9e08 100644
--- a/Makefile
+++ b/Makefile
@@ -1,8 +1,8 @@
-CFLAGS=-O3 -march=native -pipe
+CFLAGS=-O3 -march=native -flto -pipe
 LIBS=-lbluetooth
 
 OBJ = main.o
-HEADERS = main.h
+HEADERS = main.h bluetooth.h io.h
 
 %.o: %.c $(DEPS) $(HEADERS)
 	gcc -c -o $@ $< $(CFLAGS) $(LIBS)
diff --git a/bluetooth.h b/bluetooth.h
new file mode 100644
index 0000000..cc5ca79
--- /dev/null
+++ b/bluetooth.h
@@ -0,0 +1,181 @@
+#ifndef BLUETOOTH_H
+#define BLUETTOTH_H
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/l2cap.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <grp.h>
+
+int controlSock, dataSock;
+
+/*	Well known PSMs (aka defined by spec) are restricted to CAP_NET_BIND_SERVICE
+	non-priviledged process can bind only to dynamic PSMs (>= 0x1001)" */
+#define CONTROL 0x11
+#define DATA 0x13
+
+#define MAX 40
+
+bdaddr_t findBluetooth()
+	{
+	int device_id = hci_get_route(NULL);
+	int hci_sock = hci_open_dev(device_id);
+	if (device_id < 0 || hci_sock < 0)
+		{
+		perror("opening socket");
+		exit(1);
+		}
+
+	int max_rsp = 255;
+	inquiry_info *scan_info = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
+	if (!scan_info){fprintf(stderr,"Malloc failed!\n"); exit(1);}
+
+	int timeout  = 8;
+	int num_rsp = hci_inquiry(device_id, timeout, max_rsp, NULL, &scan_info, IREQ_CACHE_FLUSH);
+	if (num_rsp < 0){perror("hci_inquiry"); exit(1);}
+
+	int i = 0;
+	for (i = 0; i < num_rsp; ++i)
+		{
+		if ((scan_info[i].dev_class[0] == 0x04) && (scan_info[i].dev_class[1] == 0x01) && (scan_info[i].dev_class[2] == 0x10))//match up with class of server...
+			{
+			printf("Found device\n");
+			//we're done
+			break;
+			}
+		else
+			{
+			printf("Non matching class: %d, %d, %d\n",scan_info[i].dev_class[0],scan_info[i].dev_class[1],scan_info[i].dev_class[2]);
+			}
+		}
+	if (i == num_rsp)
+		{
+		fprintf(stderr,"Device not found in hci search!\n");
+		exit(1);
+		}
+
+	char addr[19];
+	ba2str(&(scan_info+i)->bdaddr, addr);
+	addr[18] = '\0';
+	printf("%s\n",addr);
+
+	hci_close_dev(device_id);
+
+	return (scan_info+i)->bdaddr;
+	}
+
+
+int connectPSM(int port, int *sock)
+	{
+	//Establishing a HID connection with the Wii Remote can be done on PSM 0x11 for the control pipe using the Bluetooth L2CAP protocol.
+	struct sockaddr_l2 addr = {0}, remote_addr = {0};
+	*sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sock < 0){fprintf(stderr,"Opening control socket failed!\n"); return 0;}
+
+	char buff[19];
+	socklen_t opt = sizeof(remote_addr);
+
+	// set the connection parameters (who to connect to)
+	addr.l2_psm = htobs(port);
+	bacpy(&addr.l2_bdaddr, BDADDR_ANY);//accept any, whatever
+	addr.l2_family = AF_BLUETOOTH;
+
+	/* bind to address */
+	int result = bind(*sock, (struct sockaddr *)&addr, sizeof(addr));
+	if (result < 0){fprintf(stderr,"Bind failed with error: %d & %s!\n", result, strerror(errno)); return 0;}
+
+	/* Set link mode */
+//	result = L2CAP_LM_RELIABLE | L2CAP_LM_MASTER;
+//	result = setsockopt(*sock, SOL_L2CAP, L2CAP_LM, &result, sizeof(result));
+//	if (result < 0){fprintf(stderr, "Could not set link mode\n"); return 0;}
+
+	/* Get default options */
+//	struct l2cap_options opts;
+//	memset(&opts, 0, sizeof(opts));
+  //      socklen_t optlen = sizeof(opts);
+
+    //    if (getsockopt(*sock, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0)
+//		{
+//		fprintf(stderr, "Can't get default L2CAP options: %s (%d)",strerror(errno), errno);
+//		return 0;
+//		}
+
+	/* listen for connections */
+	result = listen(*sock, 1);
+	if (result < 0){fprintf(stderr,"Listen failed!\n"); return 0;}
+
+	printf("Before\n");
+	fflush(stdout);
+
+	/* Important note: what accept returns is the fp to the client */
+	result = accept(*sock, (struct sockaddr *)&remote_addr, &opt);
+	if(result < 0){fprintf(stderr,"Failed to accept connection to control pipe!: "); perror(""); return 0;}
+
+	ba2str(&remote_addr.l2_bdaddr, buff);
+	printf("Accepted control connection from: %s\n", buff);
+
+	return result;
+	}
+
+
+int connectBluetooth(bdaddr_t address)
+	{
+	int device_id = hci_get_route(NULL);
+	int hci_sock = hci_open_dev(device_id);//todo: close when done
+	if (device_id < 0 || hci_sock < 0){fprintf(stderr,"Could not open device!\n"); return 0;}
+
+	/* Set device class to that of a wiimote */
+	if (hci_write_class_of_dev(hci_sock, 0x002504, 2000) < 0)
+		{
+		fprintf(stderr, "Can't write device class to device. Errno: %d, Err: %s\n", errno, strerror(errno));
+		return 0;
+		}
+
+/*
+	unsigned int ptype = HCI_DM1 | HCI_DM3 | HCI_DM5 | HCI_DH1 | HCI_DH3 | HCI_DH5;
+	uint16_t handle;
+
+	if (hci_create_connection(hci_sock, &address, htobs(ptype), 0, 0, &handle, 0) < 0)
+		{
+		perror("HCI create connection failed");
+		close(hci_sock);
+		return 0;
+		}
+*/
+
+	// Authenticate HCI link (without pin)
+	/*
+	if (hci_authenticate_link(hci_sock, handle, 0) < 0)
+		{
+		perror("HCI authentication failed");
+		close(hci_sock);
+		return 0;
+		}
+	*/
+
+	if ((controlSock =  connectPSM(CONTROL, &controlSock)) == 0){return 0;}
+	if ((dataSock = connectPSM(DATA, &dataSock)) == 0){return 0;}
+
+	printf("You lose\n");
+
+
+	/* drop root privs (nobody for now as getting the uid & gid from a name is messy) */
+	if (setgid(65534) != 0){fprintf(stderr,"Unable to drop group privileges!\n"); exit(1);}
+	if (setgroups(0, NULL) != 0){fprintf(stderr,"Unable to drop groups privileges!\n"); exit(1);} //We don't need any groups really
+	if (setuid(65534) != 0){fprintf(stderr,"Unable to drop user privileges!\n"); exit(1);}
+
+	/* paranoia check  */
+	if (setuid(0) != -1){fprintf(stderr,"Managed to regain root privileges?\n"); exit(1);}
+
+	return 1;
+	}
+
+void sdp()
+	{
+	//todo
+	}
+
+#endif
diff --git a/io.h b/io.h
new file mode 100644
index 0000000..7667fe0
--- /dev/null
+++ b/io.h
@@ -0,0 +1,436 @@
+#ifndef IO_H
+#define IO_H
+
+#define LEFT_B ((remoteState->dpadL) ? DPAD_LEFT:0) | ((remoteState->dpadL) ? DPAD_RIGHT:0) | ((remoteState->dpadD) ? DPAD_DOWN : 0) | ((remoteState->dpadU) ? DPAD_UP : 0) | ((remoteState->plus) ? PLUS : 0) | (OTHER0 & 0) | (OTHER1 & 0) | (UNKNOWN & 0)
+#define RIGHT_B ((remoteState->two) ? TWO : 0) | ((remoteState->one) ? ONE : 0) | ((remoteState->b) ? B : 0) | ((remoteState->b) ? A : 0) | ((remoteState->minus) ? MINUS : 0) | (OTHER2 & 0) | (OTHER3 & 0) | ((remoteState->home) ? HOME : 0)
+
+#define RUMBLE remoteState->rumble = buff[2] & 0x01
+#define ACKNOWLEDGE if (buff[2] & 0x02){remoteState->lastMode = remoteState->mode; remoteState->mode = 0x22;}//Send acknowledge output report next input report
+#define RESULT remoteState->result = 0
+
+struct state
+	{
+	bool a;
+	bool b;
+	bool plus;
+	bool minus;
+	bool home;
+	bool one;
+	bool two;
+	bool dpadL;
+	bool dpadR;
+	bool dpadU;
+	bool dpadD;
+
+	bool rumble;
+	bool continuous;
+	uint8_t mode; //data reporting mode
+	uint8_t lastMode; //previous data reporting mode
+	uint8_t battery; //battery level
+	uint8_t led;
+	bool ir;
+	bool speaker;
+
+	uint8_t eeprom[16000+1];//16000 bytes = 128kbit
+	uint8_t data[16];
+	uint8_t size;//used for data function only...
+	uint8_t result;//error code or function result
+	};
+
+struct state newState;
+
+void initState(struct state *remoteState)
+	{
+	remoteState->a = false;
+	remoteState->b = false;
+	remoteState->plus = false;
+	remoteState->minus = false;
+	remoteState->one = false;
+	remoteState->two = false;
+	remoteState->dpadL = false;
+	remoteState->dpadR = false;
+	remoteState->dpadU = false;
+	remoteState->dpadD = false;
+
+	remoteState->rumble = false;
+	remoteState->continuous = false;
+	remoteState->mode = 0x30;//data reporting mode is 0x30 by default
+	remoteState->lastMode = 0;//required to handle acknowledge output report mode
+	remoteState->battery = 142; //142 will do
+	remoteState->led = 0x10; //LED 1 is lit by default
+	remoteState->ir = false; //IR is disabled by default
+	remoteState->speaker = false;
+	remoteState->result = 0;
+
+	remoteState->size = 0;
+
+	memset(remoteState->eeprom,0,sizeof(remoteState->eeprom));//zero out the eeprom (as that's the default state)
+	/* copy the default 64 calibration bytes to the eeprom*/
+	memcpy(remoteState->eeprom, "\xa1\xaa\x8b\x99\xae\x9e\x78\x30\xa7\x74\xd3\xa1\xaa\x8b\x99\xae\x9e\x78\x30\xa7\x74\xd3\x82\x82\x82\x15\x9c\x9c\x9e\x38\x40\x3e\x82\x82\x82\
+		\x15\x9c\x9c\x9e\x38\x40\x3e\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 64);
+
+	memset(remoteState->data,0,16);
+
+	}
+
+int dataRecieved(char buff[], int length, struct state *remoteState)
+	{
+	/* In every single Output Report, bit 0 (0x01) of the first byte controls the Rumble feature. */
+
+	int dataLength;//todo: relocate
+
+	if (buff[0] != 0xa2){fprintf(stderr,"Wii sent output report with first byte %hi?\n", buff[0]); return -1;}
+
+	switch (buff[1])
+		{
+		/* rumble (a2 10 RR)*/
+		case 0x10:
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+		/* LED control (a2 11 LL)*/
+		case 0x11:
+			remoteState->led = buff[2];
+
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+		/* a2 12 TT MM : TT sets whether continuous reporting is set and MM is the mode to go into */
+		case 0x12:
+			remoteState->continuous = buff[2];
+			remoteState->mode = buff[3];
+
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+		/* camera reporting mode 0 (a2 13 04) */
+		case 0x13:
+			remoteState->ir = buff[2] & 0x4;//meant to be 1 of 2, but whatever
+
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+		/* camera reporting mode 1 */
+		case 0x1a:
+			remoteState->ir = buff[2] & 0x4; //meant to be 2 of 2, but whatever
+
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+		/* speaker enable or disable (a2 14 04)*/
+		case 0x14:
+			remoteState->speaker = buff[2] & 0x4;
+
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+		/* a2 15 00 will request the status report (and turn off rumble)*/
+		case 0x15:
+			if (buff[2] != 0){buff[length] = '\0'; fprintf(stderr,"%s unexpected\n", buff);}
+			else
+				{
+				remoteState->mode = 0x20; //set staus report mode
+				}
+
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+		/* Read data (a2 17 MM FF FF FF SS SS)*/
+		case 0x17:
+			/* clearing 0x04 in MM reads from EEPROM, while setting reads from control registers*/
+			if ((buff[2] & 0x04) && (buff[2] & 0x08))
+				{
+				remoteState->result = 0x04;//set unknown
+				RUMBLE;
+				break;
+				}
+
+			RESULT;
+
+			if (buff[2] & 0x04 || buff[2] & 0x08)
+				{
+				/* register read */
+				/* FF FF FF is the offset */
+				int offset = (buff[3] << 16 ) + (buff[4] << 8) + buff[5];//check
+				/* SS SS is the size to read in bytes */
+				int size = (buff[6] << 8) + buff[7];//check
+
+				if (offset >= 0xA20000 && offset <= 0xA30009){/* speaker settings */}
+				else if (offset >= 0xA40000 && offset <= 0xA400FF){/* Extension Controller settings and data */}
+				else if (offset >= 0xA60000 && offset <= 0xA600FF){/* Wii motion plus settings and data */}
+				else if (offset >= 0xB00000 && offset <= 0xB00033){/* IR Camera settings */}
+				else {fprintf(stderr,"Unknown offset %#06x requested\n", offset);}
+				}
+			else
+				{
+				/* EEPROM read */
+				/* FF FF FF is the offset */
+				int offset = (buff[3] << 16 ) + (buff[4] << 8) + buff[5];//check
+				/* SS SS is the size to read in bytes */
+				int size = (buff[6] << 8) + buff[7];//check
+
+				memcpy(remoteState->data, remoteState->eeprom+offset, size);
+				remoteState->size = size;
+
+				remoteState->lastMode  = remoteState->mode;
+				/* return the data read */
+				remoteState->mode = 0x21;
+				}
+
+
+			RUMBLE;
+			break;
+
+		/* Speaker data*/
+		case 0x18:
+			/* length is shifted left by 3 bytes, so shift right by 3 */
+			dataLength = (buff[2] >> 3);
+
+			memcpy(buff,buff+3,dataLength);
+
+			//playSound(buff);
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+
+		/* Speaker mute (a2 19 04)*/
+		case 0x19:
+			remoteState->speaker = buff[2] ^ 0x04;
+
+			RUMBLE;
+			ACKNOWLEDGE;
+			RESULT;
+			break;
+
+		default:
+			fprintf(stderr,"Unimplemented output report: %x\n", buff[1]);
+			break;
+		}
+
+	printf("Read %d bytes!\n",length);
+
+	return 0;
+	}
+
+int writeCommand(struct state *remoteState)
+	{
+	char buff[MAX];
+
+	int size = 0;
+	switch (remoteState->mode)
+		{
+		case 0x20:
+			/* This mode is used when The Wiimote conforms sync by writing: (a1) 20 BB BB LF 00 00 VV to the data pipe, or by request */
+			buff[0] = 0xa1;
+			buff[1] = 0x20;
+			buff[2] = 0x20;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			buff[4] = remoteState->led;
+			buff[5] = 0;
+			buff[6] = 0;
+			buff[7] = remoteState->battery;
+
+			size = 8;
+			break;
+		/* Read Memory Data - returns 1 to 16 bytes at a time */
+		case 0x21:
+			buff[0] = 0xa1;
+			buff[1] = 0x21;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			/* SE: where S is the size in bytes, minus one and E is the error flag */
+			/* 0 for no error, 7 when attempting to read a write-only register or non-connected expansion and 8 when attempting nonexistant memory addresses read. */
+			buff[4] = ((remoteState->size-1 << 4) & 0xF0) | (0 & 0x0F);//looks about right (check)
+
+			buff[5] = 0;//need to know memory address
+			buff[6] = 0;//need to know memory address
+
+			memcpy(buff+7,remoteState->data,remoteState->size);
+
+			size = 23;
+			break;
+
+		/* Acknowledge output report, return function result (It is sent when bit 1 of the first byte of any output report is set.): a1 22 BB BB RR EE*/
+		case 0x22:
+			buff[0] = 0xa1;
+			buff[2] = 0x22;
+			buff[3] = LEFT_B;
+			buff[4] = RIGHT_B;
+			buff[5] = remoteState->lastMode;//the output report to confirm
+			buff[6] = remoteState->result; //the returned result
+			size = 6;
+
+			remoteState->mode = remoteState->lastMode;//do we revert the mode?
+			break;
+
+		/* Core buttons */
+		case 0x30:
+			buff[0] = 0xa1;
+			buff[1] = 0x30;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+
+			size = 4;
+			break;
+
+		/* Core buttons and Accelerometer */
+		case 0x31:
+			buff[0] = 0xa1;
+			buff[1] = 0x31;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			/* 0x80 for approx. stillness */
+			buff[4] = 0x80;//X axis(?)
+			buff[5] = 0x80;//Y axis(?)
+			buff[6] = 0x80;//Z axis(?)
+
+			size = 7;
+			break;
+
+		/* Core buttons with 8 extension bytes */
+		case 0x32:
+			buff[0] = 0xa1;
+			buff[1] = 0x32;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			/* no extension support...yet, so just send all 0 */
+			memset(buff+4,0,8);
+
+			size = 12;
+			break;
+
+		/* Core buttons and accelerometer with 12 IR bytes */
+		case 0x33:
+			buff[0] = 0xa1;
+			buff[1] = 0x33;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			/* 0x80 for approx. stillness */
+			buff[4] = 0x80;//X axis(?)
+			buff[5] = 0x80;//Y axis(?)
+			buff[6] = 0x80;//Z axis(?)
+			/* 12 IR bytes, all 0 since no IR...yet */
+			memset(buff+7,0,12);
+
+			size = 19;
+			break;
+
+		/* core buttons with 19 extension bytes */
+		case 0x34:
+			buff[0] = 0xa1;
+			buff[1] = 0x34;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			/* 19 extension bytes, all 0, since no IR...yet */
+			memset(buff+4,0,19);
+
+			size = 23;
+			break;
+
+		/* Core buttons and Accelerometer with 16 extension bytes*/
+		case 0x35:
+			buff[0] = 0xa1;
+			buff[1] = 0x35;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			/* 16 extension bytes, all 0, since no extensions...yet */
+			memset(buff+4,0,16);
+
+			size = 20;
+			break;
+
+		/* Core buttons with 10 IR bytes and 9 extension bytes */
+		case 0x36:
+			buff[0] = 0xa1;
+			buff[1] = 0x36;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			/* 10 IR bytes */
+			memset(buff+4,0,10);
+			/* 9 extension bytes */
+			memset(buff+14,0,9);
+
+			size = 23;
+			break;
+
+		case 0x37:
+			/* Core buttons and Accelerometer with 10 IR bytes and 6 extension bytes*/
+			buff[0] = 0xa1;
+			buff[1] = 0x37;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			/* Accelerometer */
+			buff[4] = 0x80;//X axis(?)
+			buff[5] = 0x80;//Y axis(?)
+			buff[6] = 0x80;//Z axis(?)
+			/* 10 IR bytes*/
+			memset(buff+7,0,10);
+			/* 6 extension bytes */
+			memset(buff+17,0,6);
+
+			size = 23;
+			break;
+
+		/* 21 extension bytes */
+		case 0x3d:
+			buff[0] = 0xa1;
+			buff[1] = 0x3d;
+			memset(buff+2,0,21);
+
+			size = 23;
+			break;
+
+		/* Interleaved 0 - core buttons and accelerometer with 18 IR bytes */
+		case 0x3e:
+			buff[0] = 0xa1;
+			buff[1] = 0x3e;
+			/* todo: store Z values in unused bits */
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+
+			buff[4] = 0x80;//XX somewhat still(?)
+			memset(buff+5,0,18);
+
+			size = 23;
+			break;
+
+		/* Interleaved 1 - core buttons and accelerometer with 18 IR bytes */
+		case 0x3f:
+			buff[0] = 0xa1;
+			buff[1] = 0x3f;
+			/* todo: store Z values in unused bits */
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+
+			buff[4] = 0x80;//YY somewhat still(?)
+			memset(buff+5,0,18);
+
+			size = 23;
+			break;
+
+		default:
+			fprintf(stderr,"State: %hi not implemented\n", remoteState->mode);
+			break;
+		}
+
+	return write(dataSock, buff, size);
+	}
+
+#endif
diff --git a/main.c b/main.c
index 9a524c9..871b3d6 100644
--- a/main.c
+++ b/main.c
@@ -3,273 +3,24 @@
 #include <unistd.h>
 #include <string.h>
 #include <stdbool.h>
+#include <stdint.h>
 
-#include <sys/socket.h>
-#include <sys/ioctl.h>
 #include <errno.h>
 
-#include <bluetooth/bluetooth.h>
-#include <bluetooth/l2cap.h>
-#include <bluetooth/hci.h>
-#include <bluetooth/hci_lib.h>
-
 #include "main.h"
-
-int controlSock, dataSock;
-
-//#define CONTROL 0x0011
-//#define DATA 0x0013
-
-//are 0x11 or 0x13 even valid?
-//"All PSM values shall be ODD, that is, the least significant bit of the least significant octet must be 1.
-//Also, all PSM values shall have the least significant bit of the most significant octet equal to 0."
-//Looks like they are...
-
-//slapping 0x10 on here allows accept() to work, unlike 0x11, which complains of permission denied without root access...
-//Also, any server connecting to 0x11 (that's right, the server does something a client would usually do) manages to connect automagically to something, just never to this program...
-#define CONTROL 0x1011
-#define DATA 0x1013
-
-
-
-#define MAX 40
-
-struct state
-	{
-	bool a;
-	bool b;
-	bool plus;
-	bool minus;
-	bool home;
-	bool one;
-	bool two;
-	bool dpadL;
-	bool dpadR;
-	bool dpadU;
-	bool dpadD;
-
-	bool rumble;
-	bool continuous;
-	char mode; //data reporting mode
-	};
-
-struct state newState;
-
-bdaddr_t findBluetooth()
-	{
-	int device_id = hci_get_route(NULL);
-	int hci_sock = hci_open_dev(device_id);
-	if (device_id < 0 || hci_sock < 0)
-		{
-		perror("opening socket");
-		exit(1);
-		}
-
-	int max_rsp = 255;
-	inquiry_info *scan_info = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
-	if (!scan_info){fprintf(stderr,"Malloc failed!\n"); exit(1);}
-
-	int timeout  = 8;
-	int num_rsp = hci_inquiry(device_id, timeout, max_rsp, NULL, &scan_info, IREQ_CACHE_FLUSH);
-	if (num_rsp < 0){perror("hci_inquiry"); exit(1);}
-
-	int i = 0;
-	for (i = 0; i < num_rsp; ++i)
-		{
-		if ((scan_info[i].dev_class[0] == 0x04) && (scan_info[i].dev_class[1] == 0x01) && (scan_info[i].dev_class[2] == 0x10))//match up with class of server...
-			{
-			printf("Found device\n");
-			//we're done
-			break;
-			}
-		else
-			{
-			printf("Non matching class: %d, %d, %d\n",scan_info[i].dev_class[0],scan_info[i].dev_class[1],scan_info[i].dev_class[2]);
-			}
-		}
-	if (i == num_rsp)
-		{
-		fprintf(stderr,"Device not found in hci search!\n");
-		exit(1);
-		}
-
-	char addr[19];
-	ba2str(&(scan_info+i)->bdaddr, addr);
-	addr[18] = '\0';
-	printf("%s\n",addr);
-
-	hci_close_dev(device_id);
-
-	return (scan_info+i)->bdaddr;
-	}
-
-
-
-
-int connectBluetooth(bdaddr_t address)
-	{
-//	int device_id = hci_get_route(NULL);
-//	int hci_sock = hci_open_dev(device_id);
-//	if (device_id < 0 || hci_sock < 0){fprintf(stderr,"Could not open device!\n"); return 0;}
-
-	unsigned int ptype = HCI_DM1 | HCI_DM3 | HCI_DM5 | HCI_DH1 | HCI_DH3 | HCI_DH5;
-
-	uint16_t handle;
-
-	bdaddr_t bdaddr;
-
-/*
-	if (hci_create_connection(hci_sock, &address, htobs(ptype), 0, 0, &handle, 0) < 0)
-		{
-		perror("HCI create connection failed");
-		close(hci_sock);
-		return 0;
-//		exit(1);
-		}
-
-	printf("Connected!\n");
-
-	// Authenticate HCI link (without pin)
-	if (hci_authenticate_link(hci_sock, handle, 0) < 0)
-		{
-		perror("HCI authentication failed");
-//		exit(1);
-		close(hci_sock);
-		return 0;
-		}
-*/
-
-	int status;
-
-
-	//Establishing a HID connection with the Wii Remote can be done on PSM 0x11 for the control pipe using the Bluetooth L2CAP protocol.
-	struct sockaddr_l2 control = {0}, remote_addr = {0};//share the remote address struct
-	struct bt_security bt_sec;
-	controlSock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
-	if (controlSock < 0){fprintf(stderr,"Opening control socket failed!\n"); return 0;}
-
-	char buff[19];
-	socklen_t opt = sizeof(remote_addr);
-
-	// set the connection parameters (who to connect to)
-	control.l2_psm = htobs(CONTROL);
-	bacpy(&control.l2_bdaddr, BDADDR_ANY);//accept any, whatever
-	control.l2_family = AF_BLUETOOTH;
-
-	int result = bind(controlSock, (struct sockaddr *)&control, sizeof(control));
-	if (result < 0){fprintf(stderr,"Bind failed with error: %d & %s!\n", result, strerror(errno)); return 0;}
-
-
-
-	memset(&bt_sec, 0, sizeof(bt_sec));
-	bt_sec.level = BT_SECURITY_MEDIUM;
-	result = setsockopt(controlSock, SOL_BLUETOOTH, BT_SECURITY, &bt_sec, sizeof(bt_sec));
-	if (result != 0){fprintf(stderr,"Setting security level failed!\n"); return 0;}
-
-
-	result = listen(controlSock, 1);
-	if (result < 0){fprintf(stderr,"Listen failed!\n"); return 0;}
-
-
-	printf("Before\n");
-	fflush(stdout);
-
-	status = accept(controlSock, (struct sockaddr *)&remote_addr, &opt);
-	if(status < 0){fprintf(stderr,"Failed to accept connection to control pipe!: "); perror(""); return 0;}
-
-
-	ba2str(&remote_addr.l2_bdaddr, buff);
-	printf("Accepted control connection from: %s\n", buff);
-
-	/*
-                //Set receive buffer size
-		if (rcvbuf && setsockopt(nsk, SOL_SOCKET, SO_RCVBUF, &rcvbuf,sizeof(rcvbuf)) < 0)
-			{
-			fprintf(stderr("Can't set rcv buf size: %s (%d)", strerror(errno), errno);
-			exit(1);
-			}
-
-                optlen = sizeof(rcvbuf);
-                if (getsockopt(nsk, SOL_SOCKET, SO_RCVBUF, &rcvbuf, &optlen) < 0)
-			{
-			fprintf(stderr,"Can't get rcv buf size: %s (%d)", strerror(errno), errno);
-			exit(1);
-			}
-	*/
-
-	struct sockaddr_l2 data = {0}, rem_addr = {0};//share the remote address struct
-	socklen_t socklen = sizeof(rem_addr);
-
-	//Establishing a HID connection with the Wii Remote can be done on PSM 0x13 for the data pipe using the Bluetooth L2CAP protocol.
-	dataSock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
-	if (dataSock < 0){fprintf(stderr,"Opening control socket failed!\n"); return 0;}
-
-	// set the connection parameters (who to connect to)
-	data.l2_psm = htobs(DATA);
-	bacpy(&data.l2_bdaddr, BDADDR_ANY);//accept any, whatever
-	data.l2_family = AF_BLUETOOTH;
-
-	result = bind(dataSock, (struct sockaddr *)&data, sizeof(data));
-	if (result < 0){fprintf(stderr,"Bind failed with error: %d & %d!\n", result, errno); return 0;}
-
-
-	memset(&bt_sec, 0, sizeof(bt_sec));
-	bt_sec.level = BT_SECURITY_MEDIUM;
-	result = setsockopt(dataSock, SOL_BLUETOOTH, BT_SECURITY, &bt_sec, sizeof(bt_sec));
-	if (result != 0){fprintf(stderr,"Setting security level failed!\n"); return 0;}
-
-
-	result = listen(dataSock, 1);
-	if (result < 0){fprintf(stderr,"Listen failed!\n"); return 0;}
-
-
-	printf("Before 2: Electric boogaloo\n");
-	fflush(stdout);
-
-	status = accept(dataSock, (struct sockaddr *)&rem_addr, &socklen);
-	if(status < 0){fprintf(stderr,"Failed to accept connection to data pipe!: "); perror(""); return 0;}
-
-
-	ba2str(&remote_addr.l2_bdaddr, buff);
-	printf("Accepted data connection from: %s\n", buff);
-
-
-	printf("You win *claps*\n");
-	return 1;
-	}
-
-void sdp()
-	{
-	//todo
-	}
-
-void initState(struct state *remoteState)
-	{
-	remoteState->a = false;
-	remoteState->b = false;
-	remoteState->plus = false;
-	remoteState->minus = false;
-	remoteState->one = false;
-	remoteState->two = false;
-	remoteState->dpadL = false;
-	remoteState->dpadR = false;
-	remoteState->dpadU = false;
-	remoteState->dpadD = false;
-
-	remoteState->rumble = false;
-	remoteState->continuous = false;
-	remoteState->mode = 0x30;//data reporting mode is 0x30 by default
-	}
+#include "bluetooth.h"
+#include "io.h"
 
 int main(int argc, char **argv)
 {
+	if (getuid() != 0){fprintf(stderr,"Root is required to bind to the bluetooth PSM ports\n"); exit(1);}
+
 //	bdaddr_t address = findBluetooth();
 
 	bdaddr_t address;
 	str2ba("00:19:86:00:04:3F",&address);
 
-	int result = 0;
-	result = connectBluetooth(address);
+	connectBluetooth(address);
 
 	//When using a Wii Remote, all input reports are prepended with 0xa1 and all output reports are prepended with 0xa2
 	//Output reports are sent over the data pipe, which is also used to read input reports
@@ -280,114 +31,41 @@ int main(int argc, char **argv)
 	struct state *remoteState;
 	remoteState = &newState;
 	initState(remoteState);
-	remoteState->a = true;//lets assume that the A button is held down
-
 
-	int size;
 	//the wiimote appears to spam a massive amount of random garbage with 0x3X reports pre "sync".
 	//I don't implement this "feature" hoping that isn't required.
 
 
-	//The Wiimote conforms sync by writing: (a1) 20 BB BB LF 00 00 VV to the data pipe.
-	//Our data here is: a1 20 00 00 14 00 00 8d (LF == LED 1 enabled and speaker enabled), (VV, battery level 141)
-
-	//send the sync conformation report and repeat if al the bytes have not been written.
-//	sync: size = write(dataSock,"\xa1\x20\x00\x00\x14\x00\x00\x8d",8);
 	char buff[MAX+1];
 
-	sync: size = write(dataSock,"\xa1\x20\x00\x00\x14\x00\x00\x8d",8);
-	if (size < 8)
-		{
-		fprintf(stderr,"Writing connection confirmation failed with error: %d!\n", size);
-		size = read(dataSock,buff,MAX);
-		fprintf(stderr, "%d bytes read from data sock!\n", size);
-		sleep(3);
-		goto sync;
-		}
-
 
-	//the Wii must then send output report 0x12 to change the data reporting mode, as above was sent without request
-	changemode: size = read(dataSock, buff, MAX);
-	if (size < 4){fprintf(stderr,"Reading data report mode failed!\n"); goto changemode;}
-/*
-The Wii Remote has a number of different data reporting modes. Each of these modes combines certain Core data features with data from external peripherals, and sends it to the host through one of the report IDs, determined by the mode. The data format from the peripherals is determined by the peripherals themselves, all the Wii Remote controller does is pull bytes from them and send them out to the host. Due to this, certain feature combinations are not available, as there are not enough bytes for them in any of the output modes.
-
-The Data Reporting Mode is set by sending a two-byte command to Report 0x12:
-
-(a2) 12 TT MM
-
-Bit 2 of TT specifies whether continuous reporting is desired. If bit 2 (0x04) is set, the Wii Remote will send reports whether there has been any change to the data or not. Otherwise, the Wii Remote will only send an output report when the data has changed.
-
-MM specifies the Reporting Mode. Each Mode is specified by the Output Report ID that the data will be sent to.
-
-Upon powerup, the Data Reporting Mode defaults to 0x30. Following a connection or disconnection event on the Extension Port, data reporting is disabled and the Data Reporting Mode must be reset before new data can arrive.
-*/
-	if (buff[0] == 0xa2 && buff[1] == 0x12)//a2 12 06 31
-		{
-		remoteState->continuous = buff[2] & 0x04;
-		remoteState->mode = buff[3];
-		}
-	else
-		{
-		fprintf(stderr,"Wii did not sent status report 0x12. %hi%hi%hi%hi was sent instead!\n",buff[0],buff[1],buff[2],buff[3]);
-		exit(1);
-		}
+	/* as we have synced, the mode now changes to 0x20 without explanation */
+	remoteState->mode = 0x20;
 
-	printf("That's about it for now\n");
+	printf("DataSock value: %d\n", dataSock);
+	printf("ControlSock value: %d\n", controlSock);
 
 
-/*
-	char buff[MAX+1];
-	size = read(dataSock, buff, MAX);
+	int ret;
+	sleep(1);//1 second sleep before using bluetooth is bogus...
 
-	switch (size)
+	/* communication is handled via a read/write loop */
+	while (1)
 		{
-		case 3:
-			//(a2) 15 00
-			//This will request the status report (and turn off rumble)
-			if (buff[0] == 0xa2 && buff[1] == 0x15 && buff[2] == 0)
-				{
-				buttons->rumble = false;
-				//reply: (a1) 20 BB BB LF 00 00 VV
-				buff[0] = 0xa1;
-				buff[1] = 0x20;
-				buff[2] = ((buttons->dpadL) ? DPAD_LEFT:0) | ((buttons->dpadL) ? DPAD_RIGHT:0) | ((buttons->dpadD) ? DPAD_DOWN : 0) | ((buttons->dpadU) ? DPAD_UP : 0) |
-					((buttons->plus) ? PLUS : 0) | (OTHER0 & 0) | (OTHER1 & 0) | (UNKNOWN & 0);
-				buff[3] = ((buttons->two) ? TWO : 0) | ((buttons->one) ? ONE : 0) | ((buttons->b) ? B : 0) | ((buttons->b) ? A : 0) | ((buttons->minus) ? MINUS : 0)
-					| (OTHER2 & 0) | (OTHER3 & 0) | ((buttons->home) ? HOME : 0);
-				buff[4] = 0x10;//just report that LED 1 is lit for LF
-				buff[5] = 0;
-				buff[6] = 0;
-				buff[7] = 100;//Set the battery level to 100
-
-				int result = write(dataSock, buff,8);
-				if (result < 8){fprintf(stderr,"Write failed or didn't write enough"); exit(1);}
-				}
-			else
-				{
-				fprintf(stderr,"Unexpected: %hi%hi%hi",buff[0],buff[1],buff[2]);
-				}
-			break;
+		ret = read(dataSock, buff, MAX);
+		if (ret == -1){fprintf(stderr,"Read error! Errno: %d, Error: %s\n",errno,strerror(errno));}
+		else if (ret){dataRecieved(buff, ret, remoteState); printf("Actually worked!\n"); exit(1);}
+		/* May return 0, in that case we just carry on */
 
+		usleep(550000);
 
-		default:
-			fprintf(stderr,"Unimplemented report\n");
-			break;
+		ret = writeCommand(remoteState);
+		if (ret == -1){fprintf(stderr,"Write error! Errno: %d, Error: %s\n",errno,strerror(errno));}
+		else {fprintf(stderr,"Wrote %d bytes!\n",ret); printf("Actually worked!\n"); exit(0);}
+		usleep(550000);
 		}
-*/
-
 
 
-
-
-
-	/*
-	if (status == 0)
-		{
-		status = write(s, "Install Gentoo!\n", 16);
-		}
-	*/
-
-//	close(controlSock);
-//	close(dataSock);
+	close(controlSock);
+	close(dataSock);
 }
diff --git a/main.h b/main.h
index f597426..a4300d4 100644
--- a/main.h
+++ b/main.h
@@ -16,7 +16,7 @@ Bit 	Mask 	First Byte 	Second Byte
 7 	0x80 	Unknown 	Home
 */
 
-//first byte
+/* first byte */
 #define DPAD_LEFT 0x01
 #define DPAD_RIGHT 0x02
 #define DPAD_DOWN 0x04
@@ -26,7 +26,7 @@ Bit 	Mask 	First Byte 	Second Byte
 #define OTHER1 0x40
 #define UNKNOWN 0x80
 
-//second byte
+/* second byte */
 #define TWO 0x01
 #define ONE 0x02
 #define B 0x04
@@ -37,6 +37,6 @@ Bit 	Mask 	First Byte 	Second Byte
 #define HOME 0x80
 
 
-
+#define MAX 40
 
 #endif