Merge pull request #20 from rpavlik/handshake-changes

Handshake/Expansion changes

src/classic.c

@@ -49,9 +49,8 @@ static void classic_ctrl_pressed_buttons(struct classic_ctrl_t* cc, short now);
  *
  *	@return	Returns 1 if handshake was successful, 0 if not.
  */
+#define HANDSHAKE_BYTES_USED 12
 int classic_ctrl_handshake(struct wiimote_t* wm, struct classic_ctrl_t* cc, byte* data, unsigned short len) {
-	int i;
-	int offset = 0;
 
 	cc->btns = 0;
 	cc->btns_held = 0;
@@ -59,11 +58,7 @@ int classic_ctrl_handshake(struct wiimote_t* wm, struct classic_ctrl_t* cc, byte
 	cc->r_shoulder = 0;
 	cc->l_shoulder = 0;
 
-	/* decrypt data */
+	if (data[0] == 0xFF || len < HANDSHAKE_BYTES_USED) {
-	for (i = 0; i < len; ++i)
-		data[i] = (data[i] ^ 0x17) + 0x17;
-
-	if (data[offset] == 0xFF) {
 		/*
 		 *	Sometimes the data returned here is not correct.
 		 *	This might happen because the wiimote is lagging
@@ -74,7 +69,7 @@ int classic_ctrl_handshake(struct wiimote_t* wm, struct classic_ctrl_t* cc, byte
 		 *	but since the next 16 bytes are the same, just use
 		 *	those.
 		 */
-		if (data[offset + 16] == 0xFF) {
+		if (len < 17 || len < HANDSHAKE_BYTES_USED + 16 || data[16] == 0xFF) {
 			/* get the calibration data */
 			byte* handshake_buf = (byte *)malloc(EXP_HANDSHAKE_LEN * sizeof(byte));
 
@@ -83,24 +78,24 @@ int classic_ctrl_handshake(struct wiimote_t* wm, struct classic_ctrl_t* cc, byte
 
 			return 0;
 		} else
-			offset += 16;
+			data += 16;
 	}
 
 
 	/* joystick stuff */
-	cc->ljs.max.x = data[0 + offset] / 4;
+	cc->ljs.max.x = data[0] / 4;
-	cc->ljs.min.x = data[1 + offset] / 4;
+	cc->ljs.min.x = data[1] / 4;
-	cc->ljs.center.x = data[2 + offset] / 4;
+	cc->ljs.center.x = data[2] / 4;
-	cc->ljs.max.y = data[3 + offset] / 4;
+	cc->ljs.max.y = data[3] / 4;
-	cc->ljs.min.y = data[4 + offset] / 4;
+	cc->ljs.min.y = data[4] / 4;
-	cc->ljs.center.y = data[5 + offset] / 4;
+	cc->ljs.center.y = data[5] / 4;
 
-	cc->rjs.max.x = data[6 + offset] / 8;
+	cc->rjs.max.x = data[6] / 8;
-	cc->rjs.min.x = data[7 + offset] / 8;
+	cc->rjs.min.x = data[7] / 8;
-	cc->rjs.center.x = data[8 + offset] / 8;
+	cc->rjs.center.x = data[8] / 8;
-	cc->rjs.max.y = data[9 + offset] / 8;
+	cc->rjs.max.y = data[9] / 8;
-	cc->rjs.min.y = data[10 + offset] / 8;
+	cc->rjs.min.y = data[10] / 8;
-	cc->rjs.center.y = data[11 + offset] / 8;
+	cc->rjs.center.y = data[11] / 8;
 
 	/* handshake done */
 	wm->exp.type = EXP_CLASSIC;
@@ -131,13 +126,9 @@ void classic_ctrl_disconnected(struct classic_ctrl_t* cc) {
  *	@param msg		The message specified in the event packet.
  */
 void classic_ctrl_event(struct classic_ctrl_t* cc, byte* msg) {
-	int i, lx, ly, rx, ry;
+	int lx, ly, rx, ry;
 	byte l, r;
 
-	/* decrypt data */
-	for (i = 0; i < 6; ++i)
-		msg[i] = (msg[i] ^ 0x17) + 0x17;
-
 	classic_ctrl_pressed_buttons(cc, from_big_endian_uint16_t(msg + 4));
 
 	/* left/right buttons */

src/events.c

@@ -294,11 +294,29 @@ static void clear_dirty_reads(struct wiimote_t* wm) {
 	}
 }
 
+/**
+ *	@brief Handle accel data in a wiimote message.
+ *
+ *	@param wm		Pointer to a wiimote_t structure.
+ *	@param msg		The message specified in the event packet.
+ */
+static void handle_wm_accel(struct wiimote_t* wm, byte* msg) {
+	wm->accel.x = msg[2];
+	wm->accel.y = msg[3];
+	wm->accel.z = msg[4];
+
+	/* calculate the remote orientation */
+	calculate_orientation(&wm->accel_calib, &wm->accel, &wm->orient, WIIMOTE_IS_FLAG_SET(wm, WIIUSE_SMOOTHING));
+
+	/* calculate the gforces on each axis */
+	calculate_gforce(&wm->accel_calib, &wm->accel, &wm->gforce);
+}
+
 
 /**
  *	@brief Analyze the event that occurred on a wiimote.
  *
- *	@param wm		An array of pointers to wiimote_t structures.
+ *	@param wm		Pointer to a wiimote_t structure.
  *	@param event	The event that occurred.
  *	@param msg		The message specified in the event packet.
  *
@@ -319,15 +337,7 @@ void propagate_event(struct wiimote_t* wm, byte event, byte* msg) {
 			/* button - motion */
 			wiiuse_pressed_buttons(wm, msg);
 
-			wm->accel.x = msg[2];
+			handle_wm_accel(wm, msg);
-			wm->accel.y = msg[3];
-			wm->accel.z = msg[4];
-
-			/* calculate the remote orientation */
-			calculate_orientation(&wm->accel_calib, &wm->accel, &wm->orient, WIIMOTE_IS_FLAG_SET(wm, WIIUSE_SMOOTHING));
-
-			/* calculate the gforces on each axis */
-			calculate_gforce(&wm->accel_calib, &wm->accel, &wm->gforce);
 
 			break;
 		}
@@ -360,12 +370,7 @@ void propagate_event(struct wiimote_t* wm, byte event, byte* msg) {
 			/* button - motion - expansion */
 			wiiuse_pressed_buttons(wm, msg);
 
-			wm->accel.x = msg[2];
+			handle_wm_accel(wm, msg);
-			wm->accel.y = msg[3];
-			wm->accel.z = msg[4];
-
-			calculate_orientation(&wm->accel_calib, &wm->accel, &wm->orient, WIIMOTE_IS_FLAG_SET(wm, WIIUSE_SMOOTHING));
-			calculate_gforce(&wm->accel_calib, &wm->accel, &wm->gforce);
 
 			handle_expansion(wm, msg+5);
 
@@ -376,12 +381,7 @@ void propagate_event(struct wiimote_t* wm, byte event, byte* msg) {
 			/* button - motion - ir */
 			wiiuse_pressed_buttons(wm, msg);
 
-			wm->accel.x = msg[2];
+			handle_wm_accel(wm, msg);
-			wm->accel.y = msg[3];
-			wm->accel.z = msg[4];
-
-			calculate_orientation(&wm->accel_calib, &wm->accel, &wm->orient, WIIMOTE_IS_FLAG_SET(wm, WIIUSE_SMOOTHING));
-			calculate_gforce(&wm->accel_calib, &wm->accel, &wm->gforce);
 
 			/* ir */
 			calculate_extended_ir(wm, msg+5);
@@ -404,12 +404,7 @@ void propagate_event(struct wiimote_t* wm, byte event, byte* msg) {
 			/* button - motion - ir - expansion */
 			wiiuse_pressed_buttons(wm, msg);
 
-			wm->accel.x = msg[2];
+			handle_wm_accel(wm, msg);
-			wm->accel.y = msg[3];
-			wm->accel.z = msg[4];
-
-			calculate_orientation(&wm->accel_calib, &wm->accel, &wm->orient, WIIMOTE_IS_FLAG_SET(wm, WIIUSE_SMOOTHING));
-			calculate_gforce(&wm->accel_calib, &wm->accel, &wm->gforce);
 
 			handle_expansion(wm, msg+15);
 
@@ -762,6 +757,7 @@ void handshake_expansion(struct wiimote_t* wm, byte* data, uint16_t len) {
 	byte val = 0;
 	byte buf = 0x00;
 	byte* handshake_buf;
+	int gotIt = 0;
 
 	switch(wm->expansion_state) {
 		/* These two initialization writes disable the encryption */
@@ -791,31 +787,37 @@ void handshake_expansion(struct wiimote_t* wm, byte* data, uint16_t len) {
 			if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP))
 				disable_expansion(wm);
 			handshake_buf = malloc(EXP_HANDSHAKE_LEN * sizeof(byte));
-			wiiuse_read_data_cb(wm, handshake_expansion, handshake_buf, WM_EXP_MEM_CALIBR, EXP_HANDSHAKE_LEN);
-
 			/* tell the wiimote to send expansion data */
 			WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP);
+			wiiuse_read_data_cb(wm, handshake_expansion, handshake_buf, WM_EXP_MEM_CALIBR, EXP_HANDSHAKE_LEN);
 			break;
 		case 3:
 			if(!data || !len) {
 				WIIUSE_DEBUG("no handshake data received from expansion");
+				disable_expansion(wm);
 				return;
 			}
 			id = from_big_endian_uint32_t(data + 220);
 			switch(id) {
 				case EXP_ID_CODE_NUNCHUK:
-					if (nunchuk_handshake(wm, &wm->exp.nunchuk, data, len))
+					if (nunchuk_handshake(wm, &wm->exp.nunchuk, data, len)) {
 						wm->event = WIIUSE_NUNCHUK_INSERTED;
+						gotIt = 1;
+					}
 					break;
 
 				case EXP_ID_CODE_CLASSIC_CONTROLLER:
-					if (classic_ctrl_handshake(wm, &wm->exp.classic, data, len))
+					if (classic_ctrl_handshake(wm, &wm->exp.classic, data, len)) {
 						wm->event = WIIUSE_CLASSIC_CTRL_INSERTED;
+						gotIt = 1;
+					}
 					break;
 
 				case EXP_ID_CODE_GUITAR:
-					if (guitar_hero_3_handshake(wm, &wm->exp.gh3, data, len))
+					if (guitar_hero_3_handshake(wm, &wm->exp.gh3, data, len)) {
 						wm->event = WIIUSE_GUITAR_HERO_3_CTRL_INSERTED;
+						gotIt = 1;
+					}
 					break;
 
 				case EXP_ID_CODE_MOTION_PLUS:
@@ -823,11 +825,14 @@ void handshake_expansion(struct wiimote_t* wm, byte* data, uint16_t len) {
 				case EXP_ID_CODE_MOTION_PLUS_NUNCHUK:
 					/* wiiuse_motion_plus_handshake(wm, data, len); */
 					wm->event = WIIUSE_MOTION_PLUS_ACTIVATED;
+					gotIt = 1;
 					break;
 
                 case EXP_ID_CODE_WII_BOARD:
-                    if(wii_board_handshake(wm, &wm->exp.wb, data, len))
+                    if(wii_board_handshake(wm, &wm->exp.wb, data, len)) {
-                        wm->event = WIIUSE_WII_BOARD_CTRL_INSERTED;
+						wm->event = WIIUSE_WII_BOARD_CTRL_INSERTED;
+						gotIt = 1;
+					}
                     break;
 
 				default:
@@ -835,8 +840,12 @@ void handshake_expansion(struct wiimote_t* wm, byte* data, uint16_t len) {
 					break;
 			}
 			free(data);
-			WIIMOTE_DISABLE_STATE(wm,WIIMOTE_STATE_EXP_HANDSHAKE);
+			if (gotIt) {
-			WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
+				WIIMOTE_DISABLE_STATE(wm,WIIMOTE_STATE_EXP_HANDSHAKE);
+				WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
+			} else {
+				WIIUSE_WARNING("Could not handshake with expansion id: 0x%x", id);
+			}
 			wiiuse_set_ir_mode(wm);
 			wiiuse_status(wm);
 			break;
@@ -890,6 +899,7 @@ void disable_expansion(struct wiimote_t* wm) {
 
 	WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
 	wm->exp.type = EXP_NONE;
+	wm->expansion_state = 0;
 }
 
 

src/guitar_hero_3.c

@@ -51,8 +51,6 @@ static void guitar_hero_3_pressed_buttons(struct guitar_hero_3_t* gh3, short now
  *	@return	Returns 1 if handshake was successful, 0 if not.
  */
 int guitar_hero_3_handshake(struct wiimote_t* wm, struct guitar_hero_3_t* gh3, byte* data, unsigned short len) {
-	int i;
-	int offset = 0;
 
 	/*
 	 *	The good fellows that made the Guitar Hero 3 controller
@@ -65,11 +63,11 @@ int guitar_hero_3_handshake(struct wiimote_t* wm, struct guitar_hero_3_t* gh3, b
 	gh3->btns_released = 0;
 	gh3->whammy_bar = 0.0f;
 
-	/* decrypt data */
+	/*
-	for (i = 0; i < len; ++i)
+	TODO: If we're not using anything from calibration data, why are we
-		data[i] = (data[i] ^ 0x17) + 0x17;
+	even bothering here?
-
+	*/
-	if (data[offset] == 0xFF) {
+	if (data[0] == 0xFF) {
 		/*
 		 *	Sometimes the data returned here is not correct.
 		 *	This might happen because the wiimote is lagging
@@ -80,7 +78,7 @@ int guitar_hero_3_handshake(struct wiimote_t* wm, struct guitar_hero_3_t* gh3, b
 		 *	but since the next 16 bytes are the same, just use
 		 *	those.
 		 */
-		if (data[offset + 16] == 0xFF) {
+		if (data[16] == 0xFF) {
 			/* get the calibration data */
 			byte* handshake_buf = malloc(EXP_HANDSHAKE_LEN * sizeof(byte));
 
@@ -89,7 +87,7 @@ int guitar_hero_3_handshake(struct wiimote_t* wm, struct guitar_hero_3_t* gh3, b
 
 			return 0;
 		} else
-			offset += 16;
+			data += 16;
 	}
 
 	/* joystick stuff */
@@ -129,11 +127,6 @@ void guitar_hero_3_disconnected(struct guitar_hero_3_t* gh3) {
  *	@param msg		The message specified in the event packet.
  */
 void guitar_hero_3_event(struct guitar_hero_3_t* gh3, byte* msg) {
-	int i;
-
-	/* decrypt data */
-	for (i = 0; i < 6; ++i)
-		msg[i] = (msg[i] ^ 0x17) + 0x17;
 
 	guitar_hero_3_pressed_buttons(gh3, from_big_endian_uint16_t(msg + 4));
 

src/nunchuk.c

@@ -48,10 +48,8 @@
  *
  *	@return	Returns 1 if handshake was successful, 0 if not.
  */
+#define HANDSHAKE_BYTES_USED 14
 int nunchuk_handshake(struct wiimote_t* wm, struct nunchuk_t* nc, byte* data, unsigned short len) {
-	int i;
-	int offset = 0;
-
 	nc->btns = 0;
 	nc->btns_held = 0;
 	nc->btns_released = 0;
@@ -60,11 +58,7 @@ int nunchuk_handshake(struct wiimote_t* wm, struct nunchuk_t* nc, byte* data, un
 	nc->flags = &wm->flags;
 	nc->accel_calib.st_alpha = wm->accel_calib.st_alpha;
 
-	/* decrypt data */
+	if (data[0] == 0xFF || len < HANDSHAKE_BYTES_USED) {
-	for (i = 0; i < len; ++i)
-		data[i] = (data[i] ^ 0x17) + 0x17;
-
-	if (data[offset] == 0xFF) {
 		/*
 		 *	Sometimes the data returned here is not correct.
 		 *	This might happen because the wiimote is lagging
@@ -75,7 +69,7 @@ int nunchuk_handshake(struct wiimote_t* wm, struct nunchuk_t* nc, byte* data, un
 		 *	but since the next 16 bytes are the same, just use
 		 *	those.
 		 */
-		if (data[offset + 16] == 0xFF) {
+		if (len < 17 || len < HANDSHAKE_BYTES_USED + 16 || data[16] == 0xFF) {
 			/* get the calibration data */
 			byte* handshake_buf = (byte *)malloc(EXP_HANDSHAKE_LEN * sizeof(byte));
 
@@ -84,21 +78,21 @@ int nunchuk_handshake(struct wiimote_t* wm, struct nunchuk_t* nc, byte* data, un
 
 			return 0;
 		} else
-			offset += 16;
+			data += 16;
 	}
 
-	nc->accel_calib.cal_zero.x = data[offset + 0];
+	nc->accel_calib.cal_zero.x = data[0];
-	nc->accel_calib.cal_zero.y = data[offset + 1];
+	nc->accel_calib.cal_zero.y = data[1];
-	nc->accel_calib.cal_zero.z = data[offset + 2];
+	nc->accel_calib.cal_zero.z = data[2];
-	nc->accel_calib.cal_g.x = data[offset + 4];
+	nc->accel_calib.cal_g.x = data[4];
-	nc->accel_calib.cal_g.y = data[offset + 5];
+	nc->accel_calib.cal_g.y = data[5];
-	nc->accel_calib.cal_g.z = data[offset + 6];
+	nc->accel_calib.cal_g.z = data[6];
-	nc->js.max.x = data[offset + 8];
+	nc->js.max.x = data[8];
-	nc->js.min.x = data[offset + 9];
+	nc->js.min.x = data[9];
-	nc->js.center.x = data[offset + 10];
+	nc->js.center.x = data[10];
-	nc->js.max.y = data[offset + 11];
+	nc->js.max.y = data[11];
-	nc->js.min.y = data[offset + 12];
+	nc->js.min.y = data[12];
-	nc->js.center.y = data[offset + 13];
+	nc->js.center.y = data[13];
 	WIIUSE_DEBUG("Nunchuk calibration X: min %x, max %x, center %x Y: min %x, max %x, center %x",
 		nc->js.min.x, nc->js.max.x, nc->js.center.x,
 		nc->js.min.y, nc->js.max.y, nc->js.center.y);
@@ -136,11 +130,6 @@ void nunchuk_disconnected(struct nunchuk_t* nc) {
  *	@param msg		The message specified in the event packet.
  */
 void nunchuk_event(struct nunchuk_t* nc, byte* msg) {
-	int i;
-
-	/* decrypt data */
-	for (i = 0; i < 6; ++i)
-		msg[i] = (msg[i] ^ 0x17) + 0x17;
 
 	/* get button states */
 	nunchuk_pressed_buttons(nc, msg[5]);

src/wiiuse.c

@@ -354,7 +354,7 @@ int wiiuse_read_data_cb(struct wiimote_t* wm, wiiuse_read_cb read_cb, byte* buff
 
 	if (!wm || !WIIMOTE_IS_CONNECTED(wm))
 		return 0;
-	if (!buffer || !len || !read_cb)
+	if (!buffer || !len)
 		return 0;
 
 	/* make this request structure */
@@ -407,43 +407,7 @@ int wiiuse_read_data_cb(struct wiimote_t* wm, wiiuse_read_cb read_cb, byte* buff
  *	finishes.
  */
 int wiiuse_read_data(struct wiimote_t* wm, byte* buffer, unsigned int addr, uint16_t len) {
-	struct read_req_t* req;
+	return wiiuse_read_data_cb(wm, NULL, buffer, addr, len);
-
-	if (!wm || !WIIMOTE_IS_CONNECTED(wm))
-		return 0;
-	if (!buffer || !len)
-		return 0;
-
-	/* make this request structure */
-	req = (struct read_req_t*)malloc(sizeof(struct read_req_t));
-	if (req == NULL)
-		return 0;
-	req->cb = NULL;
-	req->buf = buffer;
-	req->addr = addr;
-	req->size = len;
-	req->wait = len;
-	req->dirty = 0;
-	req->next = NULL;
-
-	/* add this to the request list */
-	if (!wm->read_req) {
-		/* root node */
-		wm->read_req = req;
-
-		WIIUSE_DEBUG("Data read request can be sent out immediately.");
-
-		/* send the request out immediately */
-		wiiuse_send_next_pending_read_request(wm);
-	} else {
-		struct read_req_t* nptr = wm->read_req;
-		for (; nptr->next; nptr = nptr->next);
-		nptr->next = req;
-
-		WIIUSE_DEBUG("Added pending data read request.");
-	}
-
-	return 1;
 }