1 files changed, 98 insertions, 1 deletions

diff --git a/drivers/hid/hid-wiimote-ext.c b/drivers/hid/hid-wiimote-ext.c
index 0a1805c9b0e5..38ae87772e96 100644
--- a/drivers/hid/hid-wiimote-ext.c
+++ b/drivers/hid/hid-wiimote-ext.c
@@ -28,12 +28,14 @@ struct wiimote_ext {
 	bool mp_plugged;
 	bool motionp;
 	__u8 ext_type;
+	__u16 calib[4][3];
 };
 
 enum wiiext_type {
 	WIIEXT_NONE,		/* placeholder */
 	WIIEXT_CLASSIC,		/* Nintendo classic controller */
 	WIIEXT_NUNCHUCK,	/* Nintendo nunchuck controller */
+	WIIEXT_BALANCE_BOARD,	/* Nintendo balance board controller */
 };
 
 enum wiiext_keys {
@@ -126,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;
 
@@ -151,6 +154,28 @@ static __u8 ext_read(struct wiimote_ext *ext)
 			type = WIIEXT_NUNCHUCK;
 		else if (rmem[0] == 0x01 && rmem[1] == 0x01)
 			type = WIIEXT_CLASSIC;
+		else if (rmem[0] == 0x04 && rmem[1] == 0x02)
+			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);
@@ -204,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);
 }
 
 /*
@@ -509,6 +534,71 @@ static void handler_classic(struct wiimote_ext *ext, const __u8 *payload)
 	input_sync(ext->input);
 }
 
+static void handler_balance_board(struct wiimote_ext *ext, const __u8 *payload)
+{
+	__s32 val[4], tmp;
+	unsigned int i;
+
+	/*   Byte |  8  7  6  5  4  3  2  1  |
+	 *   -----+--------------------------+
+	 *    1   |    Top Right <15:8>      |
+	 *    2   |    Top Right  <7:0>      |
+	 *   -----+--------------------------+
+	 *    3   | Bottom Right <15:8>      |
+	 *    4   | Bottom Right  <7:0>      |
+	 *   -----+--------------------------+
+	 *    5   |     Top Left <15:8>      |
+	 *    6   |     Top Left  <7:0>      |
+	 *   -----+--------------------------+
+	 *    7   |  Bottom Left <15:8>      |
+	 *    8   |  Bottom Left  <7:0>      |
+	 *   -----+--------------------------+
+	 *
+	 * These values represent the weight-measurements of the Wii-balance
+	 * board with 16bit precision.
+	 *
+	 * The balance-board is never reported interleaved with motionp.
+	 */
+
+	val[0] = payload[0];
+	val[0] <<= 8;
+	val[0] |= payload[1];
+
+	val[1] = payload[2];
+	val[1] <<= 8;
+	val[1] |= payload[3];
+
+	val[2] = payload[4];
+	val[2] <<= 8;
+	val[2] |= payload[5];
+
+	val[3] = payload[6];
+	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]);
+	input_report_abs(ext->input, ABS_HAT1Y, val[3]);
+
+	input_sync(ext->input);
+}
+
 /* call this with state.lock spinlock held */
 void wiiext_handle(struct wiimote_data *wdata, const __u8 *payload)
 {
@@ -523,6 +613,8 @@ void wiiext_handle(struct wiimote_data *wdata, const __u8 *payload)
 		handler_nunchuck(ext, payload);
 	} else if (ext->ext_type == WIIEXT_CLASSIC) {
 		handler_classic(ext, payload);
+	} else if (ext->ext_type == WIIEXT_BALANCE_BOARD) {
+		handler_balance_board(ext, payload);
 	}
 }
 
@@ -551,6 +643,11 @@ static ssize_t wiiext_show(struct device *dev, struct device_attribute *attr,
 			return sprintf(buf, "motionp+classic\n");
 		else
 			return sprintf(buf, "classic\n");
+	} else if (type == WIIEXT_BALANCE_BOARD) {
+		if (motionp)
+			return sprintf(buf, "motionp+balanceboard\n");
+		else
+			return sprintf(buf, "balanceboard\n");
 	} else {
 		if (motionp)
 			return sprintf(buf, "motionp\n");