Merge tag 'upstream-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/jgarzik...

[~andy/linux] / drivers / hid / hid-wiimote-ext.c

diff --git a/drivers/hid/hid-wiimote-ext.c b/drivers/hid/hid-wiimote-ext.c
index ae022b863593bdbcc8587cfc002b890c3ba50573..38ae87772e96d18144a5b2e256192fd67b4bde55 100644 (file)
--- a/drivers/hid/hid-wiimote-ext.c
+++ b/drivers/hid/hid-wiimote-ext.c
@@ -28,6 +28,7 @@ struct wiimote_ext {
        bool mp_plugged;
        bool motionp;
        __u8 ext_type;
+       __u16 calib[4][3];
 };
 
 enum wiiext_type {
@@ -127,6 +128,7 @@ error:
 static __u8 ext_read(struct wiimote_ext *ext)
 {
        ssize_t ret;
+       __u8 buf[24], i, j, offs = 0;
        __u8 rmem[2], wmem;
        __u8 type = WIIEXT_NONE;
 
@@ -156,6 +158,26 @@ static __u8 ext_read(struct wiimote_ext *ext)
                        type = WIIEXT_BALANCE_BOARD;
        }
 
+       /* get balance board calibration data */
+       if (type == WIIEXT_BALANCE_BOARD) {
+               ret = wiimote_cmd_read(ext->wdata, 0xa40024, buf, 12);
+               ret += wiimote_cmd_read(ext->wdata, 0xa40024 + 12,
+                                       buf + 12, 12);
+
+               if (ret != 24) {
+                       type = WIIEXT_NONE;
+               } else {
+                       for (i = 0; i < 3; i++) {
+                               for (j = 0; j < 4; j++) {
+                                       ext->calib[j][i] = buf[offs];
+                                       ext->calib[j][i] <<= 8;
+                                       ext->calib[j][i] |= buf[offs + 1];
+                                       offs += 2;
+                               }
+                       }
+               }
+       }
+
        wiimote_cmd_release(ext->wdata);
 
        return type;
@@ -207,7 +229,7 @@ static void wiiext_worker(struct work_struct *work)
 /* schedule work only once, otherwise mark for reschedule */
 static void wiiext_schedule(struct wiimote_ext *ext)
 {
-       queue_work(system_nrt_wq, &ext->worker);
+       schedule_work(&ext->worker);
 }
 
 /*
@@ -514,7 +536,8 @@ static void handler_classic(struct wiimote_ext *ext, const __u8 *payload)
 
 static void handler_balance_board(struct wiimote_ext *ext, const __u8 *payload)
 {
-       __s32 val[4];
+       __s32 val[4], tmp;
+       unsigned int i;
 
        /*   Byte |  8  7  6  5  4  3  2  1  |
         *   -----+--------------------------+
@@ -553,6 +576,21 @@ static void handler_balance_board(struct wiimote_ext *ext, const __u8 *payload)
        val[3] <<= 8;
        val[3] |= payload[7];
 
+       /* apply calibration data */
+       for (i = 0; i < 4; i++) {
+               if (val[i] < ext->calib[i][1]) {
+                       tmp = val[i] - ext->calib[i][0];
+                       tmp *= 1700;
+                       tmp /= ext->calib[i][1] - ext->calib[i][0];
+               } else {
+                       tmp = val[i] - ext->calib[i][1];
+                       tmp *= 1700;
+                       tmp /= ext->calib[i][2] - ext->calib[i][1];
+                       tmp += 1700;
+               }
+               val[i] = tmp;
+       }
+
        input_report_abs(ext->input, ABS_HAT0X, val[0]);
        input_report_abs(ext->input, ABS_HAT0Y, val[1]);
        input_report_abs(ext->input, ABS_HAT1X, val[2]);