Changed the initial handshake to be synchronous

Made the Motion+ init a lot more robust
Code cleanup

src/events.c

@@ -164,6 +164,9 @@ int wiiuse_poll(struct wiimote_t** wm, int wiimotes) {
 				propagate_event(wm[i], wm[i]->event_buf[1], wm[i]->event_buf+2);
 				evnt += (wm[i]->event != WIIUSE_NONE);
 			} else {
+                /* send out any waiting writes */
+                wiiuse_send_next_pending_write_request(wm[i]);
+
 				idle_cycle(wm[i]);
 			}
 		}
@@ -186,6 +189,9 @@ int wiiuse_poll(struct wiimote_t** wm, int wiimotes) {
 				/* clear out the event buffer */
 				memset(wm[i]->event_buf, 0, sizeof(wm[i]->event_buf));
 			} else {
+                /* send out any waiting writes */
+                wiiuse_send_next_pending_write_request(wm[i]);
+
 				idle_cycle(wm[i]);
 			}
 		}
@@ -650,9 +656,16 @@ static void event_status(struct wiimote_t* wm, byte* msg) {
 	if (msg[2] & WM_CTRL_STATUS_BYTE1_LED_3)	led[2] = 1;
 	if (msg[2] & WM_CTRL_STATUS_BYTE1_LED_4)	led[3] = 1;
 
+    /* probe for Motion+ */
+    if(!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_MPLUS_PRESENT))
+        wiiuse_probe_motion_plus(wm);
+
 	/* is an attachment connected to the expansion port? */
 	if ((msg[2] & WM_CTRL_STATUS_BYTE1_ATTACHMENT) == WM_CTRL_STATUS_BYTE1_ATTACHMENT)
+    {
+        WIIUSE_DEBUG("Attachment detected!");
 		attachment = 1;
+    }
 
 	/* is the speaker enabled? */
 	if ((msg[2] & WM_CTRL_STATUS_BYTE1_SPEAKER_ENABLED) == WM_CTRL_STATUS_BYTE1_SPEAKER_ENABLED)
@@ -708,7 +721,6 @@ static void event_status(struct wiimote_t* wm, byte* msg) {
 	req->state = REQ_DONE;
 	/* if(req->cb!=NULL) req->cb(wm,msg,6); */
 	free(req);
-	wiiuse_send_next_pending_write_request(wm);
 }
 
 

src/io.c

@@ -32,9 +32,11 @@
  */
 #include "io.h"
 #include "ir.h"                         /* for wiiuse_set_ir_mode */
+#include "wiiuse_internal.h"
 
 #include <stdlib.h>                     /* for free, malloc */
 
+extern void propagate_event(struct wiimote_t* wm, byte event, byte* msg);
 
 static void wiiuse_disable_motion_plus2(struct wiimote_t *wm,byte *data,unsigned short len)
 {
@@ -53,6 +55,85 @@ static void wiiuse_disable_motion_plus1(struct wiimote_t *wm,byte *data,unsigned
 	wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_disable_motion_plus2);
 }
 
+ /**
+*    @brief Wait until specified report arrives and return it
+*
+*    @param wm        Pointer to a wiimote_t structure.
+*    @param data      Pre-allocated memory to store the received data
+*
+*    Synchronous/blocking, this function will not return until it receives the specified
+*    report from the Wiimote.
+*
+*/
+void wiiuse_wait_report(struct wiimote_t *wm, int report, byte *data)
+{
+    for(;;)
+    {
+        if(wiiuse_io_read(wm, data, 32) > 0)
+            if(data[1] == report)
+                break;
+    }
+}
+
+/**
+*    @brief Read memory/register data synchronously
+*
+*    @param wm        Pointer to a wiimote_t structure.
+*    @param memory    If set to non-zero, reads EEPROM, otherwise registers
+*    @param addr      Address offset to read from
+*    @param size      How many bytes to read
+*    @param data      Pre-allocated memory to store the received data
+*
+*    Synchronous/blocking read, this function will not return until it receives the specified
+*    amount of data from the Wiimote.
+*
+*/
+void wiiuse_read(struct wiimote_t *wm, byte memory, unsigned addr, unsigned short size, byte *data)
+{
+    byte pkt[8];
+    byte buf[32];
+    unsigned n_full_reports;
+    unsigned last_report;
+    byte *output;
+    int i;
+
+    /*
+     * address in big endian first, the leading byte will
+     * be overwritten (only 3 bytes are sent)
+     */
+    to_big_endian_uint32_t(pkt + 2, addr);
+
+    pkt[0] = 0x52; /* HID read command */
+    pkt[1] = 0x17; /* report 17 - read */
+    pkt[2] = (memory != 0) ? 0x00 : 0x04; /* read from registers or memory*/
+
+    /*
+     * length in big endian
+     */
+    to_big_endian_uint16_t(pkt + 6, size);
+
+    /* calculate how many 16B packets we have to get back */
+    n_full_reports = size / 16;
+    last_report = size % 16;
+    output = data;
+
+    wiiuse_io_write(wm, pkt, 8);
+
+    for(i = 0; i < n_full_reports; ++i)
+    {
+        wiiuse_wait_report(wm, 0x21, buf);
+        memmove(output, buf + 7, 16);
+        output += 16;
+    }
+
+    /* read the last incomplete packet */
+    if(last_report)
+    {
+        wiiuse_wait_report(wm, 0x21, buf);
+        memmove(output, buf + 7, last_report);
+    }
+}
+
  /**
  *	@brief Get initialization data from the wiimote.
  *
@@ -66,18 +147,93 @@ static void wiiuse_disable_motion_plus1(struct wiimote_t *wm,byte *data,unsigned
  *	The handshake will be concluded when the wiimote responds
  *	with this data.
  */
+
+#define SYNC_HANDSHAKE 1
+#ifdef SYNC_HANDSHAKE
+
+void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len)
+{
+    /* send request to wiimote for accelerometer calibration */
+    byte buf[32];
+    byte report_type;
+
+    /* step 0 - Reset wiimote */
+    {
+        //wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
+
+        WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
+        WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_CONNECTED);
+        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
+        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
+        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
+        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
+        WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS);
+
+        wiiuse_set_report_type(wm);
+        wiiuse_millisleep(500);
+
+        WIIUSE_DEBUG("Wiimote reset!\n");
+    }
+
+    /* step 1 - calibration of accelerometers */
+    {
+        struct accel_t* accel = &wm->accel_calib;
+        byte val;
+
+        wiiuse_read(wm, 1, WM_MEM_OFFSET_CALIBRATION, 8, buf);
+
+        /* received read data */
+        accel->cal_zero.x = buf[0];
+        accel->cal_zero.y = buf[1];
+        accel->cal_zero.z = buf[2];
+
+        accel->cal_g.x = buf[4] - accel->cal_zero.x;
+        accel->cal_g.y = buf[5] - accel->cal_zero.y;
+        accel->cal_g.z = buf[6] - accel->cal_zero.z;
+
+        WIIUSE_DEBUG("Calibrated wiimote acc\n");
+    }
+
+    /* step 2 - re-enable IR and ask for status */
+    {
+        WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
+        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
+
+        /* now enable IR if it was set before the handshake completed */
+         if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR))
+         {
+             WIIUSE_DEBUG("Handshake finished, enabling IR.");
+             WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
+             wiiuse_set_ir(wm, 1);
+         }
+
+         WIIUSE_DEBUG("Asking for status ...\n");
+         wm->event = WIIUSE_CONNECT;
+         wiiuse_status(wm);
+    }
+}
+
+#else
 void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
 	if (!wm)	return;
 
 	switch (wm->handshake_state) {
 		case 0:
 		{
-			/* send request to wiimote for accelerometer calibration */
 			byte* buf;
 
+            /* continous reporting off, report to buttons only */
 			WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
 			wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
 
+            WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
+            WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
+            WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
+            WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS);
+
+            wiiuse_set_report_type(wm);
+
+            /* send request to wiimote for accelerometer calibration */
 			buf = (byte*)malloc(sizeof(byte) * 8);
 			wiiuse_read_data_cb(wm, wiiuse_handshake, buf, WM_MEM_OFFSET_CALIBRATION, 7);
 			wm->handshake_state++;
@@ -143,3 +299,4 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
 		}
 	}
 }
+#endif

src/io.h

@@ -52,7 +52,10 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len);
 void wiiuse_init_platform_fields(struct wiimote_t* wm);
 void wiiuse_cleanup_platform_fields(struct wiimote_t* wm);
 
-int wiiuse_io_read(struct wiimote_t* wm);
+void wiiuse_wait_report(struct wiimote_t *wm, int report, byte *data);
+void wiiuse_read(struct wiimote_t *wm, byte memory, unsigned addr, unsigned short size, byte *data);
+
+int wiiuse_io_read(struct wiimote_t* wm, byte* buf, int len);
 int wiiuse_io_write(struct wiimote_t* wm, byte* buf, int len);
 /** @} */
 

src/io_nix.c

@@ -267,14 +267,26 @@ void wiiuse_disconnect(struct wiimote_t* wm) {
 }
 
 
-int wiiuse_io_read(struct wiimote_t* wm) {
+int wiiuse_io_read(struct wiimote_t* wm, byte* buf, int len) {
-	/* not used */
+    int rc;
-	return 0;
+
+    rc = read(wm->in_sock, buf, len);
+
+    if(rc <= 0)
+        wiiuse_disconnected(wm);
+
+    return rc;
 }
 
 
 int wiiuse_io_write(struct wiimote_t* wm, byte* buf, int len) {
-	return write(wm->out_sock, buf, len);
+    int rc;
+    rc = write(wm->out_sock, buf, len);
+
+    if(rc < 0)
+        wiiuse_disconnected(wm);
+
+    return rc;
 }
 
 void wiiuse_init_platform_fields(struct wiimote_t* wm) {

src/motion_plus.c

@@ -30,6 +30,7 @@
 
 #include "motion_plus.h"
 
+#include "wiiuse_internal.h"            /* for endian conversions */
 #include "events.h"                     /* for disable_expansion */
 #include "ir.h"                         /* for wiiuse_set_ir_mode */
 #include "nunchuk.h"                    /* for nunchuk_pressed_buttons */
@@ -42,6 +43,68 @@ static void wiiuse_calibrate_motion_plus(struct motion_plus_t *mp);
 static void calculate_gyro_rates(struct motion_plus_t* mp);
 
 
+void wiiuse_probe_motion_plus(struct wiimote_t *wm)
+{
+    byte buf[32];
+    unsigned id;
+
+    wiiuse_read(wm, 0, WM_EXP_MOTION_PLUS_IDENT, 6, buf);
+
+    /* check error code */
+    if(buf[4] & 0x0f)
+    {
+        WIIUSE_DEBUG("No Motion+ available, stopping probe.");
+        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);
+        return;
+    }
+
+    /* decode the id*/
+    id = from_big_endian_uint32_t(buf + 2);
+
+    if(id != EXP_ID_CODE_INACTIVE_MOTION_PLUS &&
+                    id != EXP_ID_CODE_NLA_MOTION_PLUS &&
+                    id != EXP_ID_CODE_NLA_MOTION_PLUS_NUNCHUK &&
+                    id != EXP_ID_CODE_NLA_MOTION_PLUS_CLASSIC)
+    {
+        /* we have read something weird */
+        WIIUSE_DEBUG("Motion+ ID doesn't match, probably not connected.");
+        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);
+        return;
+    }
+
+    WIIUSE_DEBUG("Detected inactive Motion+!");
+    WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);
+
+    /* init M+ */
+    buf[0] = 0x55;
+    wiiuse_write_data(wm, WM_EXP_MOTION_PLUS_INIT, buf, 1);
+
+    /* Init whatever is hanging on the pass-through port */
+    buf[0] = 0x55;
+    wiiuse_write_data(wm, WM_EXP_MEM_ENABLE1, buf, 1);
+
+    buf[0] = 0x00;
+    wiiuse_write_data(wm, WM_EXP_MEM_ENABLE2, buf, 1);
+
+    /* Init gyroscope data */
+    wm->exp.mp.cal_gyro.roll = 0;
+    wm->exp.mp.cal_gyro.pitch = 0;
+    wm->exp.mp.cal_gyro.yaw = 0;
+    wm->exp.mp.orient.roll = 0.0;
+    wm->exp.mp.orient.pitch = 0.0;
+    wm->exp.mp.orient.yaw = 0.0;
+    wm->exp.mp.raw_gyro_threshold = 10;
+
+    wm->exp.mp.nc = &(wm->exp.nunchuk);
+    wm->exp.mp.classic = &(wm->exp.classic);
+    wm->exp.nunchuk.flags = &wm->flags;
+
+    wm->exp.mp.ext = 0;
+
+    wiiuse_set_ir_mode(wm);
+    wiiuse_set_report_type(wm);
+}
+
 void wiiuse_motion_plus_handshake(struct wiimote_t *wm,byte *data,unsigned short len)
 {
 	uint32_t val;
@@ -133,7 +196,11 @@ void wiiuse_set_motion_plus(struct wiimote_t *wm, int status)
 {
 	byte val;
 
-	if(status && !WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
+    if(!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_MPLUS_PRESENT) ||
+                    WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP_HANDSHAKE))
+        return;
+
+    if(status)
 	{
 		WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
 		val = (status == 1) ? 0x04 : 0x05;

src/wiiuse_internal.h

@@ -172,6 +172,7 @@
 #define WM_EXP_MEM_ENABLE1			0x04A400F0
 #define WM_EXP_MEM_ENABLE2			0x04A400FB
 #define WM_EXP_MEM_CALIBR			0x04A40020
+#define WM_EXP_MOTION_PLUS_IDENT    0x04A600FA
 #define WM_EXP_MOTION_PLUS_ENABLE   		0x04A600FE
 #define WM_EXP_MOTION_PLUS_INIT     0x04A600F0
 #define WM_REG_IR				0x04B00030
@@ -209,10 +210,16 @@
 #define EXP_ID_CODE_WII_BOARD               0xA4200402
 #define EXP_ID_CODE_CLASSIC_CONTROLLER      0xA4200101
 #define EXP_ID_CODE_GUITAR                  0xA4200103
-#define EXP_ID_CODE_MOTION_PLUS             0xa4200405
+#define EXP_ID_CODE_MOTION_PLUS             0xA4200405
 #define EXP_ID_CODE_MOTION_PLUS_NUNCHUK     0xA4200505 /** Motion Plus ID in Nunchuck passthrough mode */
 #define EXP_ID_CODE_MOTION_PLUS_CLASSIC     0xA4200705 /** Motion Plus ID in Classic control. passthrough */
 
+/* decrypted M+ codes at 0x04A600FA */
+#define EXP_ID_CODE_INACTIVE_MOTION_PLUS        0xA6200005 /** Inactive Motion Plus ID */
+#define EXP_ID_CODE_NLA_MOTION_PLUS             0xA6200405 /** No longer active Motion Plus ID */
+#define EXP_ID_CODE_NLA_MOTION_PLUS_NUNCHUK     0xA6200505 /** No longer active Motion Plus ID in Nunchuck passthrough mode */
+#define EXP_ID_CODE_NLA_MOTION_PLUS_CLASSIC     0xA6200705 /** No longer active Motion Plus ID in Classic control. passthrough */
+
 #define EXP_HANDSHAKE_LEN					224
 
 /********************
@@ -239,6 +246,7 @@
 #define WIIMOTE_STATE_EXP_HANDSHAKE        0x10000 /* actual M+ connection exists but no handshake yet */
 #define WIIMOTE_STATE_EXP_EXTERN           0x20000 /* actual M+ connection exists but handshake failed */
 #define WIIMOTE_STATE_EXP_FAILED           0x40000 /* actual M+ connection exists but handshake failed */
+#define WIIMOTE_STATE_MPLUS_PRESENT        0x80000 /* Motion+ is connected */