Re: [PATCH v12 14/14] HID: nintendo: add IMU support

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).

A large part of this patch's functionality was provided by Carl Mueller.

The IMU device is blacklisted from the joydev input handler.

Signed-off-by: Daniel J. Ogorchock <djogorchock@gmail.com>
---
 drivers/hid/hid-nintendo.c | 503 ++++++++++++++++++++++++++++++++++++-
 drivers/input/joydev.c     |  10 +
 2 files changed, 501 insertions(+), 12 deletions(-)

diff --git a/drivers/hid/hid-nintendo.c b/drivers/hid/hid-nintendo.c
index 80f33ab58a5e..9bbab3bbeaf1 100644
--- a/drivers/hid/hid-nintendo.c
+++ b/drivers/hid/hid-nintendo.c

@@ -2,7 +2,7 @@
 /*
  * HID driver for Nintendo Switch Joy-Cons and Pro Controllers
  *
- * Copyright (c) 2019 Daniel J. Ogorchock <djogorchock@gmail.com>
+ * Copyright (c) 2019-2020 Daniel J. Ogorchock <djogorchock@gmail.com>
  *
  * The following resources/projects were referenced for this driver:
  *   https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering

@@ -26,6 +26,7 @@
 #include <asm/unaligned.h>
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/kernel.h>
 #include <linux/hid.h>
 #include <linux/input.h>
 #include <linux/jiffies.h>

@@ -115,6 +116,15 @@ static const u16 JC_CAL_USR_RIGHT_DATA_ADDR	= 0x801D;
 static const u16 JC_CAL_FCT_DATA_LEFT_ADDR	= 0x603d;
 static const u16 JC_CAL_FCT_DATA_RIGHT_ADDR	= 0x6046;
 
+/* SPI storage addresses of IMU factory calibration data */
+static const u16 JC_IMU_CAL_FCT_DATA_ADDR	= 0x6020;
+static const u16 JC_IMU_CAL_FCT_DATA_END	= 0x6037;
+#define JC_IMU_CAL_DATA_SIZE \
+	(JC_IMU_CAL_FCT_DATA_END - JC_IMU_CAL_FCT_DATA_ADDR + 1)
+/* SPI storage addresses of IMU user calibration data */
+static const u16 JC_IMU_CAL_USR_MAGIC_ADDR	= 0x8026;
+static const u16 JC_IMU_CAL_USR_DATA_ADDR	= 0x8028;
+
 /* 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;

@@ -125,6 +135,47 @@ static const u16 JC_MAX_DPAD_MAG		= 1;
 static const u16 JC_DPAD_FUZZ			/*= 0*/;
 static const u16 JC_DPAD_FLAT			/*= 0*/;
 
+/* Under most circumstances IMU reports are pushed every 15ms; use as default */
+static const u16 JC_IMU_DFLT_AVG_DELTA_MS	= 15;
+/* How many samples to sum before calculating average IMU report delta */
+static const u16 JC_IMU_SAMPLES_PER_DELTA_AVG	= 300;
+/* Controls how many dropped IMU packets at once trigger a warning message */
+static const u16 JC_IMU_DROPPED_PKT_WARNING	= 3;
+
+/*
+ * The controller's accelerometer has a sensor resolution of 16bits and is
+ * configured with a range of +-8000 milliGs. Therefore, the resolution can be
+ * calculated thus: (2^16-1)/(8000 * 2) = 4.096 digits per milliG
+ * Resolution per G (rather than per millliG): 4.096 * 1000 = 4096 digits per G
+ * Alternatively: 1/4096 = .0002441 Gs per digit
+ */
+static const s32 JC_IMU_MAX_ACCEL_MAG		= 32767;
+static const u16 JC_IMU_ACCEL_RES_PER_G		= 4096;
+static const u16 JC_IMU_ACCEL_FUZZ		= 10;
+static const u16 JC_IMU_ACCEL_FLAT		/*= 0*/;
+
+/*
+ * The controller's gyroscope has a sensor resolution of 16bits and is
+ * configured with a range of +-2000 degrees/second.
+ * Digits per dps: (2^16 -1)/(2000*2) = 16.38375
+ * dps per digit: 16.38375E-1 = .0610
+ *
+ * STMicro recommends in the datasheet to add 15% to the dps/digit. This allows
+ * the full sensitivity range to be saturated without clipping. This yields more
+ * accurate results, so it's the technique this driver uses.
+ * dps per digit (corrected): .0610 * 1.15 = .0702
+ * digits per dps (corrected): .0702E-1 = 14.247
+ *
+ * Now, 14.247 truncating to 14 loses a lot of precision, so we rescale the
+ * min/max range by 1000.
+ */
+static const s32 JC_IMU_PREC_RANGE_SCALE	= 1000;
+/* Note: change mag and res_per_dps if prec_range_scale is ever altered */
+static const s32 JC_IMU_MAX_GYRO_MAG		= 32767000; /* (2^16-1)*1000 */
+static const u16 JC_IMU_GYRO_RES_PER_DPS	= 14247; /* (14.247*1000) */
+static const u16 JC_IMU_GYRO_FUZZ		= 10;
+static const u16 JC_IMU_GYRO_FLAT		/*= 0*/;
+
 /* frequency/amplitude tables for rumble */
 struct joycon_rumble_freq_data {
 	u16 high;

@@ -259,6 +310,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.

@@ -306,6 +362,15 @@ 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_input_report {
 	u8 id;
 	u8 timer;

@@ -315,11 +380,11 @@ 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;
+		/* IMU input reports contain 3 samples */
+		u8 imu_raw_bytes[sizeof(struct joycon_imu_data) * 3];
+	};
 } __packed;
 
 #define JC_MAX_RESP_SIZE	(sizeof(struct joycon_input_report) + 35)

@@ -360,6 +425,13 @@ 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;
+
+	/* prevents needlessly recalculating these divisors every sample */
+	s32 imu_cal_accel_divisor[3];
+	s32 imu_cal_gyro_divisor[3];
+
 	/* power supply data */
 	struct power_supply *battery;
 	struct power_supply_desc battery_desc;

@@ -379,6 +451,16 @@ struct joycon_ctlr {
 	u16 rumble_rl_freq;
 	u16 rumble_rh_freq;
 	bool rumble_zero_amp;
+
+	/* imu */
+	struct input_dev *imu_input;
+	bool imu_first_packet_received; /* helps in initiating timestamp */
+	unsigned int imu_timestamp_us; /* timestamp we report to userspace */
+	unsigned int imu_last_pkt_ms; /* used to calc imu report delta */
+	/* the following are used to track the average imu report time delta */
+	unsigned int imu_delta_samples_count;
+	unsigned int imu_delta_samples_sum;
+	unsigned int imu_avg_delta_ms;
 };
 
 /* Helper macros for checking controller type */

@@ -563,7 +645,7 @@ static int joycon_request_spi_flash_read(struct joycon_ctlr *ctlr,
 	} else {
 		report = (struct joycon_input_report *)ctlr->input_buf;
 		/* The read data starts at the 6th byte */
-		*reply = &report->reply.data[5];
+		*reply = &report->subcmd_reply.data[5];
 	}
 	return ret;
 }

@@ -723,6 +805,94 @@ static int joycon_request_calibration(struct joycon_ctlr *ctlr)
 	return 0;
 }
 
+/*
+ * These divisors are calculated once rather than for each sample. They are only
+ * dependent on the IMU calibration values. They are used when processing the
+ * IMU input reports.
+ */
+static void joycon_calc_imu_cal_divisors(struct joycon_ctlr *ctlr)
+{
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ctlr->imu_cal_accel_divisor[i] = ctlr->accel_cal.scale[i] -
+						ctlr->accel_cal.offset[i];
+		ctlr->imu_cal_gyro_divisor[i] = ctlr->gyro_cal.scale[i] -
+						ctlr->gyro_cal.offset[i];
+	}
+}
+
+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)
+{
+	u16 imu_cal_addr = JC_IMU_CAL_FCT_DATA_ADDR;
+	u8 *raw_cal;
+	int ret;
+	int i;
+
+	/* check if user calibration exists */
+	if (!joycon_check_for_cal_magic(ctlr, JC_IMU_CAL_USR_MAGIC_ADDR)) {
+		imu_cal_addr = JC_IMU_CAL_USR_DATA_ADDR;
+		hid_info(ctlr->hdev, "using user cal for IMU\n");
+	} else {
+		hid_info(ctlr->hdev, "using factory cal for IMU\n");
+	}
+
+	/* request IMU calibration data */
+	hid_dbg(ctlr->hdev, "requesting IMU cal data\n");
+	ret = joycon_request_spi_flash_read(ctlr, imu_cal_addr,
+					    JC_IMU_CAL_DATA_SIZE, &raw_cal);
+	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;
+		}
+		joycon_calc_imu_cal_divisors(ctlr);
+		return ret;
+	}
+
+	/* IMU calibration parsing */
+	for (i = 0; i < 3; i++) {
+		int j = i * 2;
+
+		ctlr->accel_cal.offset[i] = get_unaligned_le16(raw_cal + j);
+		ctlr->accel_cal.scale[i] = get_unaligned_le16(raw_cal + j + 6);
+		ctlr->gyro_cal.offset[i] = get_unaligned_le16(raw_cal + j + 12);
+		ctlr->gyro_cal.scale[i] = get_unaligned_le16(raw_cal + j + 18);
+	}
+
+	joycon_calc_imu_cal_divisors(ctlr);
+
+	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;

@@ -749,6 +919,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)