[PATCH v10 11/12] HID: nintendo: add IMU support

["Daniel J. Ogorchock" <djogorchock@gmail.com>]

This patch adds support for the controller's IMU. The accelerometer and
gyro data are both provided to userspace using a second input device.
The devices can be associated using their uniq value (set to the
controller's MAC address).

The majority of this patch's functinoality was provided by Carl Mueller.

Signed-off-by: Daniel J. Ogorchock <djogorchock@gmail.com>
---
 drivers/hid/hid-nintendo.c | 267 +++++++++++++++++++++++++++++++++++--
 1 file changed, 259 insertions(+), 8 deletions(-)

diff --git a/drivers/hid/hid-nintendo.c b/drivers/hid/hid-nintendo.c
index 7b7a0cf3fe0b..edf2ef140cb3 100644
--- a/drivers/hid/hid-nintendo.c
+++ b/drivers/hid/hid-nintendo.c
@@ -101,12 +101,29 @@ static const u16 JC_CAL_DATA_START		= 0x603d;
 static const u16 JC_CAL_DATA_END		= 0x604e;
 #define JC_CAL_DATA_SIZE	(JC_CAL_DATA_END - JC_CAL_DATA_START + 1)
 
+/* SPI storage addresses of IMU factory calibration data */
+static const u16 JC_IMU_CAL_DATA_START		= 0x6020;
+static const u16 JC_IMU_CAL_DATA_END		= 0x6037;
+#define JC_IMU_CAL_DATA_SIZE \
+			(JC_IMU_CAL_DATA_END - JC_IMU_CAL_DATA_START + 1)
 
 /* The raw analog joystick values will be mapped in terms of this magnitude */
 static const u16 JC_MAX_STICK_MAG		= 32767;
 static const u16 JC_STICK_FUZZ			= 250;
 static const u16 JC_STICK_FLAT			= 500;
 
+/* The accel axes will be mapped in terms of this magnitude */
+static const u16 JC_MAX_ACCEL_MAG		= 32767;
+static const u16 JC_ACCEL_RES			= 4096;
+static const u16 JC_ACCEL_FUZZ			= 10;
+static const u16 JC_ACCEL_FLAT			/*= 0*/;
+
+/* The gyro axes will be mapped in terms of this magnitude */
+static const u16 JC_MAX_GYRO_MAG		= 32767;
+static const u16 JC_GYRO_RES			= 13371 / 936; /* 14 (14.285) */
+static const u16 JC_GYRO_FUZZ			= 10;
+static const u16 JC_GYRO_FLAT			/*= 0*/;
+
 /* frequency/amplitude tables for rumble */
 struct joycon_rumble_freq_data {
 	u16 high;
@@ -234,6 +251,11 @@ struct joycon_stick_cal {
 	s32 center;
 };
 
+struct joycon_imu_cal {
+	s16 offset[3];
+	s16 scale[3];
+};
+
 /*
  * All the controller's button values are stored in a u32.
  * They can be accessed with bitwise ANDs.
@@ -281,6 +303,19 @@ struct joycon_subcmd_reply {
 	u8 data[0]; /* will be at most 35 bytes */
 } __packed;
 
+struct joycon_imu_data {
+	s16 accel_x;
+	s16 accel_y;
+	s16 accel_z;
+	s16 gyro_x;
+	s16 gyro_y;
+	s16 gyro_z;
+} __packed;
+
+struct joycon_imu_report {
+	struct joycon_imu_data data[3];
+} __packed;
+
 struct joycon_input_report {
 	u8 id;
 	u8 timer;
@@ -290,11 +325,10 @@ struct joycon_input_report {
 	u8 right_stick[3];
 	u8 vibrator_report;
 
-	/*
-	 * If support for firmware updates, gyroscope data, and/or NFC/IR
-	 * are added in the future, this can be swapped for a union.
-	 */
-	struct joycon_subcmd_reply reply;
+	union {
+		struct joycon_subcmd_reply subcmd_reply;
+		struct joycon_imu_report imu_report;
+	};
 } __packed;
 
 #define JC_MAX_RESP_SIZE	(sizeof(struct joycon_input_report) + 35)
@@ -334,6 +368,9 @@ struct joycon_ctlr {
 	struct joycon_stick_cal right_stick_cal_x;
 	struct joycon_stick_cal right_stick_cal_y;
 
+	struct joycon_imu_cal accel_cal;
+	struct joycon_imu_cal gyro_cal;
+
 	/* power supply data */
 	struct power_supply *battery;
 	struct power_supply_desc battery_desc;
@@ -352,6 +389,10 @@ struct joycon_ctlr {
 	u16 rumble_lh_freq;
 	u16 rumble_rl_freq;
 	u16 rumble_rh_freq;
+
+	/* imu */
+	struct input_dev *imu_input;
+	int timestamp;
 };
 
 static int __joycon_hid_send(struct hid_device *hdev, u8 *data, size_t len)
@@ -547,7 +588,7 @@ static int joycon_request_calibration(struct joycon_ctlr *ctlr)
 	}
 
 	report = (struct joycon_input_report *)ctlr->input_buf;
-	raw_cal = &report->reply.data[5];
+	raw_cal = &report->subcmd_reply.data[5];
 
 	/* left stick calibration parsing */
 	cal_x = &ctlr->left_stick_cal_x;
@@ -601,6 +642,85 @@ static int joycon_request_calibration(struct joycon_ctlr *ctlr)
 	return 0;
 }
 
+static const s16 DFLT_ACCEL_OFFSET /*= 0*/;
+static const s16 DFLT_ACCEL_SCALE = 16384;
+static const s16 DFLT_GYRO_OFFSET /*= 0*/;
+static const s16 DFLT_GYRO_SCALE  = 13371;
+static int joycon_request_imu_calibration(struct joycon_ctlr *ctlr)
+{
+	struct joycon_subcmd_request *req;
+	u8 buffer[sizeof(*req) + 5] = { 0 };
+	struct joycon_input_report *report;
+	u8 *data;
+	u8 *raw_cal;
+	int ret;
+	int i;
+
+	/* request IMU calibration data */
+	req = (struct joycon_subcmd_request *)buffer;
+	req->subcmd_id = JC_SUBCMD_SPI_FLASH_READ;
+	data = req->data;
+	data[0] = 0xFF & JC_IMU_CAL_DATA_START;
+	data[1] = 0xFF & (JC_IMU_CAL_DATA_START >> 8);
+	data[2] = 0xFF & (JC_IMU_CAL_DATA_START >> 16);
+	data[3] = 0xFF & (JC_IMU_CAL_DATA_START >> 24);
+	data[4] = JC_IMU_CAL_DATA_SIZE;
+
+	hid_dbg(ctlr->hdev, "requesting IMU cal data\n");
+	ret = joycon_send_subcmd(ctlr, req, 5, HZ);
+
+	if (ret) {
+		hid_warn(ctlr->hdev,
+			 "Failed to read IMU cal, using defaults; ret=%d\n",
+			 ret);
+
+		for (i = 0; i < 3; i++) {
+			ctlr->accel_cal.offset[i] = DFLT_ACCEL_OFFSET;
+			ctlr->accel_cal.scale[i] = DFLT_ACCEL_SCALE;
+			ctlr->gyro_cal.offset[i] = DFLT_GYRO_OFFSET;
+			ctlr->gyro_cal.scale[i] = DFLT_GYRO_SCALE;
+		}
+		return ret;
+	}
+
+	report = (struct joycon_input_report *)ctlr->input_buf;
+	raw_cal = &report->subcmd_reply.data[5];
+
+	/* IMU calibration parsing */
+	for (i = 0; i < 3; i++) {
+		int j = i * 2;
+
+		ctlr->accel_cal.offset[i] = raw_cal[j + 0] |
+						((s16)raw_cal[j + 1] << 8);
+		ctlr->accel_cal.scale[i] = raw_cal[j + 6] |
+						((s16)raw_cal[j + 7] << 8);
+		ctlr->gyro_cal.offset[i] = raw_cal[j + 12] |
+						((s16)raw_cal[j + 13] << 8);
+		ctlr->gyro_cal.scale[i] = raw_cal[j + 18] |
+						((s16)raw_cal[j + 19] << 8);
+	}
+
+	hid_dbg(ctlr->hdev, "IMU calibration:\n"
+			    "a_o[0]=%d a_o[1]=%d a_o[2]=%d\n"
+			    "a_s[0]=%d a_s[1]=%d a_s[2]=%d\n"
+			    "g_o[0]=%d g_o[1]=%d g_o[2]=%d\n"
+			    "g_s[0]=%d g_s[1]=%d g_s[2]=%d\n",
+			    ctlr->accel_cal.offset[0],
+			    ctlr->accel_cal.offset[1],
+			    ctlr->accel_cal.offset[2],
+			    ctlr->accel_cal.scale[0],
+			    ctlr->accel_cal.scale[1],
+			    ctlr->accel_cal.scale[2],
+			    ctlr->gyro_cal.offset[0],
+			    ctlr->gyro_cal.offset[1],
+			    ctlr->gyro_cal.offset[2],
+			    ctlr->gyro_cal.scale[0],
+			    ctlr->gyro_cal.scale[1],
+			    ctlr->gyro_cal.scale[2]);
+
+	return 0;
+}
+
 static int joycon_set_report_mode(struct joycon_ctlr *ctlr)
 {
 	struct joycon_subcmd_request *req;
@@ -627,6 +747,19 @@ static int joycon_enable_rumble(struct joycon_ctlr *ctlr, bool enable)
 	return joycon_send_subcmd(ctlr, req, 1, HZ/4);
 }
 
+static int joycon_enable_imu(struct joycon_ctlr *ctlr, bool enable)
+{
+	struct joycon_subcmd_request *req;
+	u8 buffer[sizeof(*req) + 1] = { 0 };
+
+	req = (struct joycon_subcmd_request *)buffer;
+	req->subcmd_id = JC_SUBCMD_ENABLE_IMU;
+	req->data[0] = enable ? 0x01 : 0x00;
+
+	hid_dbg(ctlr->hdev, "%s IMU\n", enable ? "enabling" : "disabling");
+	return joycon_send_subcmd(ctlr, req, 1, HZ);
+}
+
 static s32 joycon_map_stick_val(struct joycon_stick_cal *cal, s32 val)
 {
 	s32 center = cal->center;
@@ -771,6 +904,54 @@ static void joycon_parse_report(struct joycon_ctlr *ctlr,
 		spin_unlock_irqrestore(&ctlr->lock, flags);
 		wake_up(&ctlr->wait);
 	}
+
+	/* parse IMU data if present */
+	if (rep->id == JC_INPUT_IMU_DATA) {
+		struct joycon_imu_data *imu_data = rep->imu_report.data;
+		struct input_dev *idev = ctlr->imu_input;
+		int i;
+		int value[6];
+
+		for (i = 0; i < 3; i++) { /* there are 3 reports */
+			ctlr->timestamp += 5000; /* each is 5 ms apart */
+			input_event(idev, EV_MSC, MSC_TIMESTAMP,
+				    ctlr->timestamp);
+
+			/*
+			 * Rather than convert to floats, we adjust by
+			 * calibration factors and then multiply by the default
+			 * scaling values. This way, the consumer only needs to
+			 * divide by the default scale values.
+			 */
+			value[0] = (imu_data[i].gyro_x -
+				    ctlr->gyro_cal.offset[0]) *
+				    DFLT_GYRO_SCALE / ctlr->gyro_cal.scale[0];
+			value[1] = (imu_data[i].gyro_y -
+				    ctlr->gyro_cal.offset[1]) *
+				    DFLT_GYRO_SCALE / ctlr->gyro_cal.scale[1];
+			value[2] = (imu_data[i].gyro_z -
+				    ctlr->gyro_cal.offset[2]) *
+				    DFLT_GYRO_SCALE / ctlr->gyro_cal.scale[2];
+
+			value[3] = (imu_data[i].accel_x -
+				    ctlr->accel_cal.offset[0]) *
+				    DFLT_ACCEL_SCALE / ctlr->accel_cal.scale[0];
+			value[4] = (imu_data[i].accel_y -
+				    ctlr->accel_cal.offset[1]) *
+				    DFLT_ACCEL_SCALE / ctlr->accel_cal.scale[1];
+			value[5] = (imu_data[i].accel_z -
+				    ctlr->accel_cal.offset[2]) *
+				    DFLT_ACCEL_SCALE / ctlr->accel_cal.scale[2];
+
+			input_report_abs(idev, ABS_RX, value[0]);
+			input_report_abs(idev, ABS_RY, value[1]);
+			input_report_abs(idev, ABS_RZ, value[2]);
+			input_report_abs(idev, ABS_X, value[3]);
+			input_report_abs(idev, ABS_Y, value[4]);
+			input_report_abs(idev, ABS_Z, value[5]);
+			input_sync(idev);
+		}
+	}
 }
 
 static void joycon_rumble_worker(struct work_struct *work)
@@ -950,6 +1131,7 @@ static int joycon_input_create(struct joycon_ctlr *ctlr)
 {
 	struct hid_device *hdev;
 	const char *name;
+	const char *imu_name;
 	int ret;
 	int i;
 
@@ -958,12 +1140,15 @@ static int joycon_input_create(struct joycon_ctlr *ctlr)
 	switch (hdev->product) {
 	case USB_DEVICE_ID_NINTENDO_PROCON:
 		name = "Nintendo Switch Pro Controller";
+		imu_name = "Nintendo Switch Pro Controller IMU";
 		break;
 	case USB_DEVICE_ID_NINTENDO_JOYCONL:
 		name = "Nintendo Switch Left Joy-Con";
+		imu_name = "Nintendo Switch Left Joy-Con IMU";
 		break;
 	case USB_DEVICE_ID_NINTENDO_JOYCONR:
 		name = "Nintendo Switch Right Joy-Con";
+		imu_name = "Nintendo Switch Right Joy-Con IMU";
 		break;
 	default: /* Should be impossible */
 		hid_err(hdev, "Invalid hid product\n");
@@ -1029,6 +1214,55 @@ static int joycon_input_create(struct joycon_ctlr *ctlr)
 	if (ret)
 		return ret;
 
+	/* configure the imu input device */
+	ctlr->imu_input = devm_input_allocate_device(&hdev->dev);
+	if (!ctlr->imu_input)
+		return -ENOMEM;
+
+	ctlr->imu_input->id.bustype = hdev->bus;
+	ctlr->imu_input->id.vendor = hdev->vendor;
+	ctlr->imu_input->id.product = hdev->product;
+	ctlr->imu_input->id.version = hdev->version;
+	ctlr->imu_input->uniq = ctlr->mac_addr_str;
+	ctlr->imu_input->name = imu_name;
+	input_set_drvdata(ctlr->imu_input, ctlr);
+
+	/* configure imu axes */
+	input_set_abs_params(ctlr->imu_input, ABS_X,
+			     -JC_MAX_ACCEL_MAG, JC_MAX_ACCEL_MAG,
+			     JC_ACCEL_FUZZ, JC_ACCEL_FLAT);
+	input_set_abs_params(ctlr->imu_input, ABS_Y,
+			     -JC_MAX_ACCEL_MAG, JC_MAX_ACCEL_MAG,
+			     JC_ACCEL_FUZZ, JC_ACCEL_FLAT);
+	input_set_abs_params(ctlr->imu_input, ABS_Z,
+			     -JC_MAX_ACCEL_MAG, JC_MAX_ACCEL_MAG,
+			     JC_ACCEL_FUZZ, JC_ACCEL_FLAT);
+	input_abs_set_res(ctlr->imu_input, ABS_X, JC_ACCEL_RES);
+	input_abs_set_res(ctlr->imu_input, ABS_Y, JC_ACCEL_RES);
+	input_abs_set_res(ctlr->imu_input, ABS_Z, JC_ACCEL_RES);
+
+	input_set_abs_params(ctlr->imu_input, ABS_RX,
+			     -JC_MAX_GYRO_MAG, JC_MAX_GYRO_MAG,
+			     JC_GYRO_FUZZ, JC_GYRO_FLAT);
+	input_set_abs_params(ctlr->imu_input, ABS_RY,
+			     -JC_MAX_GYRO_MAG, JC_MAX_GYRO_MAG,
+			     JC_GYRO_FUZZ, JC_GYRO_FLAT);
+	input_set_abs_params(ctlr->imu_input, ABS_RZ,
+			     -JC_MAX_GYRO_MAG, JC_MAX_GYRO_MAG,
+			     JC_GYRO_FUZZ, JC_GYRO_FLAT);
+
+	input_abs_set_res(ctlr->imu_input, ABS_RX, JC_GYRO_RES);
+	input_abs_set_res(ctlr->imu_input, ABS_RY, JC_GYRO_RES);
+	input_abs_set_res(ctlr->imu_input, ABS_RZ, JC_GYRO_RES);
+
+	__set_bit(EV_MSC, ctlr->imu_input->evbit);
+	__set_bit(MSC_TIMESTAMP, ctlr->imu_input->mscbit);
+	__set_bit(INPUT_PROP_ACCELEROMETER, ctlr->imu_input->propbit);
+
+	ret = input_register_device(ctlr->imu_input);
+	if (ret)
+		return ret;
+
 	return 0;
 }
 
@@ -1288,7 +1522,7 @@ static int joycon_read_mac(struct joycon_ctlr *ctlr)
 	report = (struct joycon_input_report *)ctlr->input_buf;
 
 	for (i = 4, j = 0; j < 6; i++, j++)
-		ctlr->mac_addr[j] = report->reply.data[i];
+		ctlr->mac_addr[j] = report->subcmd_reply.data[i];
 
 	ctlr->mac_addr_str = devm_kasprintf(&ctlr->hdev->dev, GFP_KERNEL,
 					    "%02X:%02X:%02X:%02X:%02X:%02X",
@@ -1343,7 +1577,7 @@ static int joycon_ctlr_handle_event(struct joycon_ctlr *ctlr, u8 *data,
 			    data[0] != JC_INPUT_SUBCMD_REPLY)
 				break;
 			report = (struct joycon_input_report *)data;
-			if (report->reply.id == ctlr->subcmd_ack_match)
+			if (report->subcmd_reply.id == ctlr->subcmd_ack_match)
 				match = true;
 			break;
 		default:
@@ -1469,6 +1703,16 @@ static int nintendo_hid_probe(struct hid_device *hdev,
 		hid_warn(hdev, "Analog stick positions may be inaccurate\n");
 	}
 
+	/* get IMU calibration data, and parse it */
+	ret = joycon_request_imu_calibration(ctlr);
+	if (ret) {
+		/*
+		 * We can function with default calibration, but it may be
+		 * inaccurate. Provide a warning, and continue on.
+		 */
+		hid_warn(hdev, "Unable to read IMU calibration data\n");
+	}
+
 	/* Set the reporting mode to 0x30, which is the full report mode */
 	ret = joycon_set_report_mode(ctlr);
 	if (ret) {
@@ -1483,6 +1727,13 @@ static int nintendo_hid_probe(struct hid_device *hdev,
 		goto err_mutex;
 	}
 
+	/* Enable the IMU */
+	ret = joycon_enable_imu(ctlr, true);
+	if (ret) {
+		hid_err(hdev, "Failed to enable the IMU; ret=%d\n", ret);
+		goto err_mutex;
+	}
+
 	ret = joycon_read_mac(ctlr);
 	if (ret) {
 		hid_err(hdev, "Failed to retrieve controller MAC; ret=%d\n",
-- 
2.24.1