improved modes and support for memory reads enough to get past wiiuse initHEADmaster

4 files changed, 328 insertions, 124 deletions

diff --git a/Makefile b/Makefile
index d1a9e08..bab5208 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 CFLAGS=-O3 -march=native -flto -pipe
-LIBS=-lbluetooth
+LIBS=-lbluetooth -lSDL2
 
 OBJ = main.o
 HEADERS = main.h bluetooth.h io.h
diff --git a/bluetooth.h b/bluetooth.h
index cc5ca79..7dcec13 100644
--- a/bluetooth.h
+++ b/bluetooth.h
@@ -12,13 +12,74 @@
 
 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)" */
+/* 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
 
+
+int set_flush_timeout(bdaddr_t *ba, int timeout)
+	{
+	int ret = 0, device;
+	struct hci_conn_info_req *connection_handle = 0;
+	struct hci_request rq = {0};
+
+	struct {
+	uint16_t handle;
+	uint16_t flush_timeout;
+	} cmd_param;
+
+	struct {
+	uint8_t status;
+	uint16_t handle;
+	} cmd_response;
+
+	/* find the connection handle to the specified bluetooth device */
+	connection_handle = (struct hci_conn_info_req*) malloc(sizeof(struct hci_conn_info_req) + sizeof(struct hci_conn_info));
+	if (!connection_handle){fprintf(stderr, "Malloc failed!\n"); exit(1);}
+
+	bacpy(&connection_handle->bdaddr, ba);
+	connection_handle->type = ACL_LINK;
+	device = hci_open_dev(hci_get_route(&connection_handle->bdaddr));
+	if(device < 0)
+		{
+		ret = device;
+		goto cleanup;
+		}
+	ret = ioctl(device, HCIGETCONNINFO, (unsigned long) connection_handle);
+	if (ret){goto cleanup;}
+
+	/* build a command packet to send to the bluetooth microcontroller */
+	cmd_param.handle = connection_handle->conn_info->handle;
+	cmd_param.flush_timeout = htobs(timeout);
+	rq.ogf = OGF_HOST_CTL;
+	rq.ocf = 0x28;
+	rq.cparam = &cmd_param;
+	rq.clen = sizeof(cmd_param);
+	rq.rparam = &cmd_response;
+	rq.rlen = sizeof(cmd_response);
+	rq.event = EVT_CMD_COMPLETE;
+
+	/* send the command and wait for the response */
+	ret = hci_send_req(device, &rq, 0);
+	if (ret){goto cleanup;}
+
+	if (cmd_response.status)
+		{
+		ret = -1;
+		errno = bt_error(cmd_response.status);
+		}
+
+	cleanup:
+	free(connection_handle);
+	if (device >= 0){close(device);}
+	return ret;
+}
+
 bdaddr_t findBluetooth()
 	{
 	int device_id = hci_get_route(NULL);
@@ -33,7 +94,7 @@ bdaddr_t findBluetooth()
 	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 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);}
 
@@ -68,56 +129,45 @@ bdaddr_t findBluetooth()
 	}
 
 
-int connectPSM(int port, int *sock)
+int connectPSM(int port)
 	{
-	//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);
+	int 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)
+	/* set the socket timeout  (not accepted...)*/
+	/*
+	struct timeval tv;
+	tv.tv_sec = 1;
+	tv.tv_usec = 0;
+	setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
+	*/
+
+	/* set the connection parameters (who to connect to) */
 	addr.l2_psm = htobs(port);
-	bacpy(&addr.l2_bdaddr, BDADDR_ANY);//accept any, whatever
+	bacpy(&addr.l2_bdaddr, BDADDR_ANY);//accept any "server"
 	addr.l2_family = AF_BLUETOOTH;
 
 	/* bind to address */
-	int result = bind(*sock, (struct sockaddr *)&addr, sizeof(addr));
+	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);
+	result = listen(sock, 1);
 	if (result < 0){fprintf(stderr,"Listen failed!\n"); return 0;}
 
-	printf("Before\n");
-	fflush(stdout);
+	/* The sockfd that accept() returns is our IO pipe */
+	int io = accept(sock, (struct sockaddr *)&remote_addr, &opt);
+	if (result < 0){fprintf(stderr,"Failed to accept connection to control pipe!: "); perror(""); return 0;}
 
-	/* 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;}
 
+	char buff[19];
 	ba2str(&remote_addr.l2_bdaddr, buff);
-	printf("Accepted control connection from: %s\n", buff);
+	printf("Accepted connection from: %s\n", buff);
 
-	return result;
+	return io;
 	}
 
 
@@ -156,10 +206,11 @@ int connectBluetooth(bdaddr_t address)
 		}
 	*/
 
-	if ((controlSock =  connectPSM(CONTROL, &controlSock)) == 0){return 0;}
-	if ((dataSock = connectPSM(DATA, &dataSock)) == 0){return 0;}
+	controlSock = connectPSM(CONTROL);
+	if (!controlSock){return 0;}
 
-	printf("You lose\n");
+	dataSock = connectPSM(DATA);
+	if (!dataSock){return 0;}
 
 
 	/* drop root privs (nobody for now as getting the uid & gid from a name is messy) */
@@ -167,7 +218,7 @@ int connectBluetooth(bdaddr_t address)
 	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  */
+	/* paranoia check*/
 	if (setuid(0) != -1){fprintf(stderr,"Managed to regain root privileges?\n"); exit(1);}
 
 	return 1;
diff --git a/io.h b/io.h
index 7667fe0..6e6c4e7 100644
--- a/io.h
+++ b/io.h
@@ -4,10 +4,12 @@
 #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 RUMBLE remoteState->rumble = *(buff+2) & 0x01
+#define ACKNOWLEDGE if (buff[2] & 0x01){remoteState->lastMode = remoteState->mode; remoteState->mode = 0x22; fprintf(stderr, "Send acknowlege next input report\n"); }//Send acknowledge output report next input report
 #define RESULT remoteState->result = 0
 
+bool skipWrite = false;
+
 struct state
 	{
 	bool a;
@@ -24,16 +26,19 @@ struct state
 
 	bool rumble;
 	bool continuous;
-	uint8_t mode; //data reporting mode
-	uint8_t lastMode; //previous data reporting mode
-	uint8_t battery; //battery level
+	bool changed;
+	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 size;//number of bytes being returned
+	unsigned addressBytes;//the 2 least significant bytes of the absolute memory address of the first byte of data returned
+
 	uint8_t result;//error code or function result
 	};
 
@@ -54,6 +59,7 @@ void initState(struct state *remoteState)
 
 	remoteState->rumble = false;
 	remoteState->continuous = false;
+	remoteState->changed = true;
 	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
@@ -70,18 +76,20 @@ void initState(struct state *remoteState)
 		\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)
+int dataRecieved(uint8_t *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;}
+	fprintf(stderr, "Recieved: %02x (%02x)", *buff, *(buff+1));
+	for (int i = 2; i < length; ++i){fprintf(stderr," %02x",*(buff+i));}
+	fprintf(stderr, ", Length: %d\n", length);
 
-	switch (buff[1])
+	if (*buff != 0xa2){fprintf(stderr,"Wii sent output report with first byte %c?\n", *buff);}
+
+	switch (*(buff+1))
 		{
 		/* rumble (a2 10 RR)*/
 		case 0x10:
@@ -90,9 +98,9 @@ int dataRecieved(char buff[], int length, struct state *remoteState)
 			RESULT;
 			break;
 
-		/* LED control (a2 11 LL)*/
+		/* LED control (a2 11 LL) */
 		case 0x11:
-			remoteState->led = buff[2];
+			remoteState->led = *(buff+2);
 
 			RUMBLE;
 			ACKNOWLEDGE;
@@ -101,8 +109,10 @@ int dataRecieved(char buff[], int length, struct state *remoteState)
 
 		/* 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];
+			remoteState->continuous = *(buff+2);
+			remoteState->mode = *(buff+3);
+			/* Set that the remote state has been changed */
+			remoteState->changed = true;
 
 			RUMBLE;
 			ACKNOWLEDGE;
@@ -111,36 +121,40 @@ int dataRecieved(char buff[], int length, struct state *remoteState)
 
 		/* camera reporting mode 0 (a2 13 04) */
 		case 0x13:
-			remoteState->ir = buff[2] & 0x4;//meant to be 1 of 2, but whatever
+			remoteState->ir = *(buff+2) & 0x4;//meant to be 1 of 2, but whatever
 
+			fprintf(stderr, "IR camera enable step 1 of 2.\n");
 			RUMBLE;
-			ACKNOWLEDGE;
+//			ACKNOWLEDGE;
 			RESULT;
 			break;
 
 		/* camera reporting mode 1 */
 		case 0x1a:
-			remoteState->ir = buff[2] & 0x4; //meant to be 2 of 2, but whatever
+			remoteState->ir = *(buff+2) & 0x4; //meant to be 2 of 2, but whatever
 
+			fprintf(stderr, "IR camera enable step 2 of 2.\n");
 			RUMBLE;
-			ACKNOWLEDGE;
+//			ACKNOWLEDGE;
 			RESULT;
+
 			break;
 
-		/* speaker enable or disable (a2 14 04)*/
+		/* speaker enable or disable (a2 14 04) */
 		case 0x14:
-			remoteState->speaker = buff[2] & 0x4;
+			remoteState->speaker = *(buff+2) & 0x4;
 
 			RUMBLE;
 			ACKNOWLEDGE;
 			RESULT;
 			break;
 
-		/* a2 15 00 will request the status report (and turn off rumble)*/
+		/* 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);}
+			if (*(buff+2) != 0){*(buff+length) = '\0'; fprintf(stderr,"%s unexpected\n", buff+2);}
 			else
 				{
+				remoteState->lastMode = remoteState->mode;
 				remoteState->mode = 0x20; //set staus report mode
 				}
 
@@ -149,11 +163,74 @@ int dataRecieved(char buff[], int length, struct state *remoteState)
 			RESULT;
 			break;
 
+		/* a2 16 MM FF FF FF SS DD DD DD DD DD DD DD DD DD DD DD DD DD DD DD DD */
+		/* The meaning of the bytes is the same as during reads, except that size can be a maximum of 16 bytes (as there is only space for that much data),
+		and the actual data to write follows (the DD bytes), padded out to 16 bytes. */
+		/* FF FF FF is the offset, and SS SS is the size to read in bytes (both in big-endian format). Bit 2 (0x04) of MM selects the address space.
+		Clearing this bit results in reading from EEPROM Memory, while setting it results in reading from the control registers. Setting bit 3 (0x08) also works to access
+		registers, but setting both results in errors. */
+		/* Some kind of acknowledgement is received on Input Report 0x22. This has not been investigated yet. */
+		case 0x16:
+			/* setting 0x04 or 0x08 in MM reads from control registers, but setting both results in errors */
+			if ((*(buff+2) & 0x04) && (*(buff+2) & 0x08))
+				{
+				fprintf(stderr, "Wii set both 0x04 and 0x08?\n");
+				remoteState->result = 0x04;//set unknown
+				RUMBLE;
+				break;
+				}
+
+			RESULT;
+
+			/* FF FF FF is the offset */
+			unsigned offsetWrite = (*(buff+3) << 16) | (*(buff+4) << 8) | *(buff+5);
+			/* SS is the size to write in bytes */
+			int writeSize = *(buff+6);
+
+			if (*(buff+2) & 0x04 || *(buff+2) & 0x08)
+				{
+				fprintf(stderr, "Register write!\n");
+				fprintf(stderr, "Memory offset: 0x%x, Write length: %d\n", offsetWrite, writeSize);
+
+				if (offsetWrite >= 0xA20000 && offsetWrite <= 0xA30009){/* Speaker settings */}
+
+				/* Extension Controller settings and data */
+				else if (offsetWrite >= 0xA40000 && offsetWrite <= 0xA400FF)
+					{
+					switch (offsetWrite)
+						{
+						/* Writing 0x55 to 0xa400f0 disables MotionPlus and also resets wiimote, our implementation is just to skip our write and go back to read */
+						case 0xa400f0:
+							if (writeSize == 1 && *(buff+7) == 0x55){skipWrite = true;}
+							break;
+						}
+					}
+				else if (offsetWrite >= 0xA60000 && offsetWrite <= 0xA600FF){/* Wii motion plus settings and data */}
+				else if (offsetWrite >= 0xB00000 && offsetWrite <= 0xB00033){/* IR Camera settings */}
+				else {fprintf(stderr,"Unknown offset %#06x requested\n", offsetWrite);}
+				}
+			else
+				{
+				fprintf(stderr, "EEPROM write!\n");
+				/* EEPROM write */
+				/* FF FF FF is the offset */
+
+				memcpy(remoteState->eeprom+offsetWrite, buff+7, writeSize);
+
+				remoteState->lastMode = remoteState->mode;
+				/* confirm the data write */
+				remoteState->mode = 0x22;//check
+				}
+
+			RUMBLE;
+			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))
+			/* setting 0x04 or 0x08 in MM reads from registers, while setting both causes an error */
+			if ((*(buff+2) & 0x04) && (*(buff+2) & 0x08))
 				{
+				fprintf(stderr, "Remote state is unknown\n");
 				remoteState->result = 0x04;//set unknown
 				RUMBLE;
 				break;
@@ -161,44 +238,71 @@ int dataRecieved(char buff[], int length, struct state *remoteState)
 
 			RESULT;
 
-			if (buff[2] & 0x04 || buff[2] & 0x08)
+			/* FF FF FF is the offset */
+			unsigned offset = (*(buff+3) << 16 ) + (*(buff+4) << 8) + *(buff+5);
+			/* SS SS is the size to read in bytes */
+			int size = (*(buff+6) << 8) + *(buff+7);
+
+			remoteState->size = size;
+
+			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
+				fprintf(stderr, "Register read; Memory offset: 0x%x, Read length: %d\n", offset, size);
 
 				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 */}
+				/* Extension Controller settings and data */
+				else if (offset >= 0xA40000 && offset <= 0xA400FF)
+					{
+
+					}
+
+				/* Wii motion plus settings and data */
+				else if (offset >= 0xA60000 && offset <= 0xA600FF)
+				{
+					switch (offset)
+						{
+						case 0xA600FA:
+							if (size == 6)
+								{
+								/* If a Motion+ adaptor is available, 6 bytes of data from the address will be returned, we just return 0x07 for not attached */
+								fprintf(stderr, "Return Motion+ not connected\n");
+								remoteState->result = 0x07;
+								remoteState->size = 0;
+								}
+						}
+				}
+
 				else if (offset >= 0xB00000 && offset <= 0xB00033){/* IR Camera settings */}
-				else {fprintf(stderr,"Unknown offset %#06x requested\n", offset);}
+				else {fprintf(stderr,"Unknown Read %#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
+				fprintf(stderr, "EEPROM read; Memory offset: 0x%x, Read length: %d\n", offset, size);
 
 				memcpy(remoteState->data, remoteState->eeprom+offset, size);
 				remoteState->size = size;
+				/* pad rest with 0 if read size is not 16 */
+				if (size < 16)
+					{
+					memset(remoteState->data+size, 0, 16-size);
+					}
 
-				remoteState->lastMode  = remoteState->mode;
-				/* return the data read */
-				remoteState->mode = 0x21;
 				}
 
+			/* the 2 least significant bytes of the absolute memory address of the first byte of data is returned */
+			remoteState->addressBytes = offset & 0xFFFF;
+			remoteState->lastMode = remoteState->mode;
+			remoteState->mode = 0x21;
 
 			RUMBLE;
 			break;
 
-		/* Speaker data*/
+		/* Speaker data */
 		case 0x18:
 			/* length is shifted left by 3 bytes, so shift right by 3 */
-			dataLength = (buff[2] >> 3);
+			dataLength = (*(buff+2) >> 3);
 
 			memcpy(buff,buff+3,dataLength);
 
@@ -209,9 +313,9 @@ int dataRecieved(char buff[], int length, struct state *remoteState)
 			break;
 
 
-		/* Speaker mute (a2 19 04)*/
+		/* Speaker mute (a2 19 04) */
 		case 0x19:
-			remoteState->speaker = buff[2] ^ 0x04;
+			remoteState->speaker = *(buff+2) ^ 0x04;
 
 			RUMBLE;
 			ACKNOWLEDGE;
@@ -219,18 +323,16 @@ int dataRecieved(char buff[], int length, struct state *remoteState)
 			break;
 
 		default:
-			fprintf(stderr,"Unimplemented output report: %x\n", buff[1]);
+			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];
+	uint8_t buff[MAX];
 
 	int size = 0;
 	switch (remoteState->mode)
@@ -247,41 +349,73 @@ int writeCommand(struct state *remoteState)
 			buff[6] = 0;
 			buff[7] = remoteState->battery;
 
+			/* Go back to previous mode */
+			remoteState->mode = remoteState->lastMode;
+
 			size = 8;
 			break;
+
 		/* Read Memory Data - returns 1 to 16 bytes at a time */
+		/* (a1) 21 BB BB SE AA AA DD DD DD DD DD DD DD DD DD DD DD DD DD DD DD DD */
 		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
+			/* If return is last return or bytes to return are 16 or less, change back to previous mode */
+			if (remoteState->size <= 16)
+				{
+				remoteState->mode = remoteState->lastMode;
+				/* S (high nybble of SE) is the size in bytes, minus one, for the current data packet.
+				This is 0xf (16 bytes) for all but the last packet, where it might be less if the requested number of bytes is not a multiple of 16. */
+
+				/* E (low nybble of SE) 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) | (remoteState->result & 0x0F);//looks about right (check)
+				}
+
+			else 	{
+				buff[4] = ((16-1 << 4) & 0xF0) | (remoteState->result & 0x0F);//looks about right (check)
+				}
+
+			/* AA AA are the 2 least significant bytes of the absolute memory address of the first byte of data returned */
+			/* AA AA increases by 16 each time each 16 byte memory read */
+			buff[5] = remoteState->addressBytes & 0xFF00;
+			buff[6] = remoteState->addressBytes & 0x00FF;
+			remoteState->addressBytes += 16;
+
+			memcpy(buff+7, remoteState->data, 16);
 
-			memcpy(buff+7,remoteState->data,remoteState->size);
+			/* as we are just about to return 16 bytes, reduce total count by 16 (less than 16 bytes return possible, but value isn't used then) */
+			remoteState->size -= 16;
 
 			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*/
+		/* 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
+			buff[1] = 0x22;
+			buff[2] = LEFT_B;
+			buff[3] = RIGHT_B;
+			buff[4] = remoteState->lastMode;//the output report to confirm
+			buff[5] = remoteState->result; //the returned result
 			size = 6;
 
-			remoteState->mode = remoteState->lastMode;//do we revert the mode?
+			fprintf(stderr, "Acknowlegment sent: %02x %02x %02x %02x %02x %02x\n", buff[0], buff[1], buff[2], buff[3], buff[4], buff[5], buff[6]);
+			remoteState->mode = remoteState->lastMode;//revert mode
 			break;
 
 		/* Core buttons */
 		case 0x30:
+			if (remoteState->changed){remoteState->changed = false; skipWrite = true;}
+
+			SDL_PumpEvents();
+			const uint8_t *keyBoardState = SDL_GetKeyboardState(NULL);//get keyboard state
+			if (keyBoardState[SDL_SCANCODE_A]){fprintf(stderr, "A pressed\n"); remoteState->a = true; skipWrite = false;}
+			else {remoteState->a = false;}
+
 			buff[0] = 0xa1;
 			buff[1] = 0x30;
 			buff[2] = LEFT_B;
@@ -344,7 +478,7 @@ int writeCommand(struct state *remoteState)
 			size = 23;
 			break;
 
-		/* Core buttons and Accelerometer with 16 extension bytes*/
+		/* Core buttons and Accelerometer with 16 extension bytes */
 		case 0x35:
 			buff[0] = 0xa1;
 			buff[1] = 0x35;
@@ -371,7 +505,7 @@ int writeCommand(struct state *remoteState)
 			break;
 
 		case 0x37:
-			/* Core buttons and Accelerometer with 10 IR bytes and 6 extension bytes*/
+			/* Core buttons and Accelerometer with 10 IR bytes and 6 extension bytes */
 			buff[0] = 0xa1;
 			buff[1] = 0x37;
 			buff[2] = LEFT_B;
@@ -380,7 +514,7 @@ int writeCommand(struct state *remoteState)
 			buff[4] = 0x80;//X axis(?)
 			buff[5] = 0x80;//Y axis(?)
 			buff[6] = 0x80;//Z axis(?)
-			/* 10 IR bytes*/
+			/* 10 IR bytes */
 			memset(buff+7,0,10);
 			/* 6 extension bytes */
 			memset(buff+17,0,6);
@@ -430,6 +564,10 @@ int writeCommand(struct state *remoteState)
 			break;
 		}
 
+	fprintf(stderr, "Sent: %02x (%02x)", buff[0], buff[1]);
+	for (int i = 2; i < size; ++i){fprintf(stderr," %02x",buff[i]);}
+	fprintf(stderr, ", Length: %d\n", size);
+
 	return write(dataSock, buff, size);
 	}
 
diff --git a/main.c b/main.c
index 871b3d6..543965f 100644
--- a/main.c
+++ b/main.c
@@ -4,42 +4,52 @@
 #include <string.h>
 #include <stdbool.h>
 #include <stdint.h>
+#include <sys/ioctl.h>
 
 #include <errno.h>
+#include <SDL2/SDL.h>
 
 #include "main.h"
 #include "bluetooth.h"
 #include "io.h"
 
-int main(int argc, char **argv)
+int main(int argc, char *argv[])
 {
-	if (getuid() != 0){fprintf(stderr,"Root is required to bind to the bluetooth PSM ports\n"); exit(1);}
+	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);
+	str2ba("no",&address);
 
-	connectBluetooth(address);
+	int result = connectBluetooth(address);
+	if (!result){fprintf(stderr,"Connecting to bluetooth failed!\n"); exit(1);}
 
-	//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
 
-	//input reports are reports from the Wii remote to the Wii and output reports are reports from the Wii to the Wii remote
+	SDL_Init(0);
+
+
+
+	/* 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.
+	 * The control pipe can be used for certain reports on original Wii remotes, but such is bad practice.
+	 * Input reports are reports from the Wii remote to the Wii and output reports are reports from the Wii to the Wii remote
+	 */
 
 
 	struct state *remoteState;
 	remoteState = &newState;
 	initState(remoteState);
 
-	//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 appears to spam a massive amount of random garbage with 0x3X reports pre "sync".
+	 * Such "feature" isn't implemented, hopefully no "server" expects it.
+	 */
 
-	char buff[MAX+1];
+	uint8_t *buff = malloc(MAX+1);
+	if (!buff){fprintf(stderr, "Malloc failed!\n"); exit(1);}
 
 
-	/* as we have synced, the mode now changes to 0x20 without explanation */
+	/* When sync "succeeds", the mode changes to 0x20 without explanation */
 	remoteState->mode = 0x20;
 
 	printf("DataSock value: %d\n", dataSock);
@@ -47,25 +57,30 @@ int main(int argc, char **argv)
 
 
 	int ret;
-	sleep(1);//1 second sleep before using bluetooth is bogus...
 
 	/* communication is handled via a read/write loop */
 	while (1)
 		{
 		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 */
+		else if (ret){dataRecieved(buff, ret, remoteState);}
+		/* read() May return 0, in that case we just carry on */
 
-		usleep(550000);
+		usleep(1000);
+
+		if (!skipWrite)
+			{
+			ret = writeCommand(remoteState);
+			if (ret == -1){fprintf(stderr,"Write error! Errno: %d, Error: %s\n",errno,strerror(errno));}
+			else {fprintf(stderr,"Actually wrote %d bytes!\n",ret);}
+			usleep(1000);
+			}
+		else {skipWrite = false; fprintf(stderr, "Write skipped\n");}
 
-		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);
-		}
 
+		}
 
+	free(buff);
 	close(controlSock);
 	close(dataSock);
 }