HID: wiimote: Parse motion+ data

Motion+ reports rotation gyro data which we report to userspace as ABS_RX/Y/Z
values. The device reports them either in fast or slow mode. We adjust the
values to get a linear scale so userspace does not need to know about slow and
fast mode.

The motion+ also reports whether an extension is connected to it. We keep track
of this value and reinitialize the extensions if an extension is plugged or
unplugged.

Signed-off-by: David Herrmann <dh.herrmann@googlemail.com>
Signed-off-by: Jiri Kosina <jkosina@suse.cz>

1 files changed, 70 insertions, 0 deletions

diff --git a/drivers/hid/hid-wiimote-ext.c b/drivers/hid/hid-wiimote-ext.c
index ecc7b7b9b171..ceec0cef3268 100644
--- a/drivers/hid/hid-wiimote-ext.c
+++ b/drivers/hid/hid-wiimote-ext.c
@@ -25,6 +25,7 @@ struct wiimote_ext {
         atomic_t opened;
         atomic_t mp_opened;
         bool plugged;
+        bool mp_plugged;
         bool motionp;
         __u8 ext_type;
 };
@@ -182,6 +183,10 @@ void wiiext_event(struct wiimote_data *wdata, bool plugged)
                 return;
 
         wdata->ext->plugged = plugged;
+
+        if (!plugged)
+                wdata->ext->mp_plugged = false;
+
         /*
          * We need to call wiiext_schedule(wdata->ext) here, however, the
          * extension initialization logic is not fully understood and so
@@ -206,6 +211,63 @@ bool wiiext_active(struct wiimote_data *wdata)
 
 static void handler_motionp(struct wiimote_ext *ext, const __u8 *payload)
 {
+        __s32 x, y, z;
+        bool plugged;
+
+        /*        |   8    7    6    5    4    3 |  2  |  1  |
+         *   -----+------------------------------+-----+-----+
+         *    1   |               Yaw Speed <7:0>            |
+         *    2   |              Roll Speed <7:0>            |
+         *    3   |             Pitch Speed <7:0>            |
+         *   -----+------------------------------+-----+-----+
+         *    4   |       Yaw Speed <13:8>       | Yaw |Pitch|
+         *   -----+------------------------------+-----+-----+
+         *    5   |      Roll Speed <13:8>       |Roll | Ext |
+         *   -----+------------------------------+-----+-----+
+         *    6   |     Pitch Speed <13:8>       |  1  |  0  |
+         *   -----+------------------------------+-----+-----+
+         * The single bits Yaw, Roll, Pitch in the lower right corner specify
+         * whether the wiimote is rotating fast (0) or slow (1). Speed for slow
+         * roation is 440 deg/s and for fast rotation 2000 deg/s. To get a
+         * linear scale we multiply by 2000/440 = ~4.5454 which is 18 for fast
+         * and 9 for slow.
+         * If the wiimote is not rotating the sensor reports 2^13 = 8192.
+         * Ext specifies whether an extension is connected to the motionp.
+         */
+
+        x = payload[0];
+        y = payload[1];
+        z = payload[2];
+
+        x |= (((__u16)payload[3]) << 6) & 0xff00;
+        y |= (((__u16)payload[4]) << 6) & 0xff00;
+        z |= (((__u16)payload[5]) << 6) & 0xff00;
+
+        x -= 8192;
+        y -= 8192;
+        z -= 8192;
+
+        if (!(payload[3] & 0x02))
+                x *= 18;
+        else
+                x *= 9;
+        if (!(payload[4] & 0x02))
+                y *= 18;
+        else
+                y *= 9;
+        if (!(payload[3] & 0x01))
+                z *= 18;
+        else
+                z *= 9;
+
+        input_report_abs(ext->mp_input, ABS_RX, x);
+        input_report_abs(ext->mp_input, ABS_RY, y);
+        input_report_abs(ext->mp_input, ABS_RZ, z);
+        input_sync(ext->mp_input);
+
+        plugged = payload[5] & 0x01;
+        if (plugged != ext->mp_plugged)
+                ext->mp_plugged = plugged;
 }
 
 static void handler_nunchuck(struct wiimote_ext *ext, const __u8 *payload)
@@ -368,6 +430,14 @@ int wiiext_init(struct wiimote_data *wdata)
         ext->mp_input->id.version = wdata->hdev->version;
         ext->mp_input->name = WIIMOTE_NAME " Motion+";
 
+        set_bit(EV_ABS, ext->mp_input->evbit);
+        set_bit(ABS_RX, ext->mp_input->absbit);
+        set_bit(ABS_RY, ext->mp_input->absbit);
+        set_bit(ABS_RZ, ext->mp_input->absbit);
+        input_set_abs_params(ext->mp_input, ABS_RX, -160000, 160000, 4, 8);
+        input_set_abs_params(ext->mp_input, ABS_RY, -160000, 160000, 4, 8);
+        input_set_abs_params(ext->mp_input, ABS_RZ, -160000, 160000, 4, 8);
+
         ret = input_register_device(ext->mp_input);
         if (ret) {
                 input_free_device(ext->mp_input);