HID: wiimote: add Motion Plus extension module

[deliverable/linux.git] / drivers / hid / hid-wiimote-modules.c

diff --git a/drivers/hid/hid-wiimote-modules.c b/drivers/hid/hid-wiimote-modules.c
index daeb679f98ffbb22f5c7f43e1daea497784f9c2e..aee1b2caae130dba91498225465614951fdc9873 100644 (file)
--- a/drivers/hid/hid-wiimote-modules.c
+++ b/drivers/hid/hid-wiimote-modules.c
@@ -1475,11 +1475,155 @@ static const struct wiimod_ops wiimod_bboard = {
 
 /*
  * Motion Plus
+ * The Motion Plus extension provides rotation sensors (gyro) as a small
+ * extension device for Wii Remotes. Many devices have them built-in so
+ * you cannot see them from the outside.
+ * Motion Plus extensions are special because they are on a separate extension
+ * port and allow other extensions to be used simultaneously. This is all
+ * handled by the Wiimote Core so we don't have to deal with it.
  */
 
+static void wiimod_mp_in_mp(struct wiimote_data *wdata, const __u8 *ext)
+{
+       __s32 x, y, z;
+
+       /*        |   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.
+        * which is parsed by wiimote-core.
+        */
+
+       x = ext[0];
+       y = ext[1];
+       z = ext[2];
+
+       x |= (((__u16)ext[3]) << 6) & 0xff00;
+       y |= (((__u16)ext[4]) << 6) & 0xff00;
+       z |= (((__u16)ext[5]) << 6) & 0xff00;
+
+       x -= 8192;
+       y -= 8192;
+       z -= 8192;
+
+       if (!(ext[3] & 0x02))
+               x *= 18;
+       else
+               x *= 9;
+       if (!(ext[4] & 0x02))
+               y *= 18;
+       else
+               y *= 9;
+       if (!(ext[3] & 0x01))
+               z *= 18;
+       else
+               z *= 9;
+
+       input_report_abs(wdata->mp, ABS_RX, x);
+       input_report_abs(wdata->mp, ABS_RY, y);
+       input_report_abs(wdata->mp, ABS_RZ, z);
+       input_sync(wdata->mp);
+}
+
+static int wiimod_mp_open(struct input_dev *dev)
+{
+       struct wiimote_data *wdata = input_get_drvdata(dev);
+       unsigned long flags;
+
+       spin_lock_irqsave(&wdata->state.lock, flags);
+       wdata->state.flags |= WIIPROTO_FLAG_MP_USED;
+       wiiproto_req_drm(wdata, WIIPROTO_REQ_NULL);
+       __wiimote_schedule(wdata);
+       spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+       return 0;
+}
+
+static void wiimod_mp_close(struct input_dev *dev)
+{
+       struct wiimote_data *wdata = input_get_drvdata(dev);
+       unsigned long flags;
+
+       spin_lock_irqsave(&wdata->state.lock, flags);
+       wdata->state.flags &= ~WIIPROTO_FLAG_MP_USED;
+       wiiproto_req_drm(wdata, WIIPROTO_REQ_NULL);
+       __wiimote_schedule(wdata);
+       spin_unlock_irqrestore(&wdata->state.lock, flags);
+}
+
+static int wiimod_mp_probe(const struct wiimod_ops *ops,
+                          struct wiimote_data *wdata)
+{
+       int ret;
+
+       wdata->mp = input_allocate_device();
+       if (!wdata->mp)
+               return -ENOMEM;
+
+       input_set_drvdata(wdata->mp, wdata);
+       wdata->mp->open = wiimod_mp_open;
+       wdata->mp->close = wiimod_mp_close;
+       wdata->mp->dev.parent = &wdata->hdev->dev;
+       wdata->mp->id.bustype = wdata->hdev->bus;
+       wdata->mp->id.vendor = wdata->hdev->vendor;
+       wdata->mp->id.product = wdata->hdev->product;
+       wdata->mp->id.version = wdata->hdev->version;
+       wdata->mp->name = WIIMOTE_NAME " Motion Plus";
+
+       set_bit(EV_ABS, wdata->mp->evbit);
+       set_bit(ABS_RX, wdata->mp->absbit);
+       set_bit(ABS_RY, wdata->mp->absbit);
+       set_bit(ABS_RZ, wdata->mp->absbit);
+       input_set_abs_params(wdata->mp,
+                            ABS_RX, -16000, 16000, 4, 8);
+       input_set_abs_params(wdata->mp,
+                            ABS_RY, -16000, 16000, 4, 8);
+       input_set_abs_params(wdata->mp,
+                            ABS_RZ, -16000, 16000, 4, 8);
+
+       ret = input_register_device(wdata->mp);
+       if (ret)
+               goto err_free;
+
+       return 0;
+
+err_free:
+       input_free_device(wdata->mp);
+       wdata->mp = NULL;
+       return ret;
+}
+
+static void wiimod_mp_remove(const struct wiimod_ops *ops,
+                            struct wiimote_data *wdata)
+{
+       if (!wdata->mp)
+               return;
+
+       input_unregister_device(wdata->mp);
+       wdata->mp = NULL;
+}
+
 const struct wiimod_ops wiimod_mp = {
        .flags = 0,
        .arg = 0,
+       .probe = wiimod_mp_probe,
+       .remove = wiimod_mp_remove,
+       .in_mp = wiimod_mp_in_mp,
 };
 
 /* module table */