From 492ba955c1f7b8fdc3d87b6e4765c7a5db5f7657 Mon Sep 17 00:00:00 2001 From: David Herrmann Date: Thu, 17 Nov 2011 14:12:03 +0100 Subject: [PATCH] HID: wiimote: Add extension initializers The wiimote extension registers are not fully understood, so we always disable all extensions on extension-port events. Then we reinitialize and reidentify them and activate all requested extensions. Signed-off-by: David Herrmann Signed-off-by: Jiri Kosina --- drivers/hid/hid-wiimote-ext.c | 81 ++++++++++++++++++++++++++++++++++- 1 file 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); } -- 2.34.1