1 files changed, 79 insertions, 2 deletions

diff --git a/drivers/hid/hid-wiimote-ext.c b/drivers/hid/hid-wiimote-ext.c
index 3e3e1fc8d838..233bdfe3205b 100644
--- a/drivers/hid/hid-wiimote-ext.c
+++ b/drivers/hid/hid-wiimote-ext.c
@@ -37,30 +37,107 @@ enum wiiext_type {
 static void ext_disable(struct wiimote_ext *ext)
 {
 	unsigned long flags;
+	__u8 wmem = 0x55;
+
+	if (!wiimote_cmd_acquire(ext->wdata)) {
+		wiimote_cmd_write(ext->wdata, 0xa400f0, &wmem, sizeof(wmem));
+		wiimote_cmd_release(ext->wdata);
+	}
 
 	spin_lock_irqsave(&ext->wdata->state.lock, flags);
 	ext->motionp = false;
 	ext->ext_type = WIIEXT_NONE;
+	wiiproto_req_drm(ext->wdata, WIIPROTO_REQ_NULL);
 	spin_unlock_irqrestore(&ext->wdata->state.lock, flags);
 }
 
 static bool motionp_read(struct wiimote_ext *ext)
 {
-	return false;
+	__u8 rmem[2], wmem;
+	ssize_t ret;
+	bool avail = false;
+
+	if (wiimote_cmd_acquire(ext->wdata))
+		return false;
+
+	/* initialize motion plus */
+	wmem = 0x55;
+	ret = wiimote_cmd_write(ext->wdata, 0xa600f0, &wmem, sizeof(wmem));
+	if (ret)
+		goto error;
+
+	/* read motion plus ID */
+	ret = wiimote_cmd_read(ext->wdata, 0xa600fe, rmem, 2);
+	if (ret == 2 || rmem[1] == 0x5)
+		avail = true;
+
+error:
+	wiimote_cmd_release(ext->wdata);
+	return avail;
 }
 
 static __u8 ext_read(struct wiimote_ext *ext)
 {
-	return WIIEXT_NONE;
+	ssize_t ret;
+	__u8 rmem[2], wmem;
+	__u8 type = WIIEXT_NONE;
+
+	if (!ext->plugged)
+		return WIIEXT_NONE;
+
+	if (wiimote_cmd_acquire(ext->wdata))
+		return WIIEXT_NONE;
+
+	/* initialize extension */
+	wmem = 0x55;
+	ret = wiimote_cmd_write(ext->wdata, 0xa400f0, &wmem, sizeof(wmem));
+	if (!ret) {
+		/* disable encryption */
+		wmem = 0x0;
+		wiimote_cmd_write(ext->wdata, 0xa400fb, &wmem, sizeof(wmem));
+	}
+
+	/* read extension ID */
+	ret = wiimote_cmd_read(ext->wdata, 0xa400fe, rmem, 2);
+	if (ret == 2) {
+		if (rmem[0] == 0 && rmem[1] == 0)
+			type = WIIEXT_NUNCHUCK;
+		else if (rmem[0] == 0x01 && rmem[1] == 0x01)
+			type = WIIEXT_CLASSIC;
+	}
+
+	wiimote_cmd_release(ext->wdata);
+
+	return type;
 }
 
 static void ext_enable(struct wiimote_ext *ext, bool motionp, __u8 ext_type)
 {
 	unsigned long flags;
+	__u8 wmem;
+	int ret;
+
+	if (motionp) {
+		if (wiimote_cmd_acquire(ext->wdata))
+			return;
+
+		if (ext_type == WIIEXT_CLASSIC)
+			wmem = 0x07;
+		else if (ext_type == WIIEXT_NUNCHUCK)
+			wmem = 0x05;
+		else
+			wmem = 0x04;
+
+		ret = wiimote_cmd_write(ext->wdata, 0xa600fe, &wmem, sizeof(wmem));
+		wiimote_cmd_release(ext->wdata);
+		if (ret)
+			return;
+	}
 
 	spin_lock_irqsave(&ext->wdata->state.lock, flags);
 	ext->motionp = motionp;
 	ext->ext_type = ext_type;
+	wiiproto_req_drm(ext->wdata, WIIPROTO_REQ_NULL);
 	spin_unlock_irqrestore(&ext->wdata->state.lock, flags);
 }