Re: [PATCH v4 05/13] HID: playstation: add DualSense accelerometer and gyroscope support.

[Benjamin Tissoires <benjamin.tissoires@redhat.com>]

Hi Roderick,

On 1/18/21 12:44 AM, Roderick Colenbrander wrote:
> From: Roderick Colenbrander <roderick.colenbrander@sony.com>
> 
> The DualSense features an accelerometer and gyroscope. The data is
> embedded into the main HID input reports. Expose both sensors through
> through a separate evdev node.
> 
> Signed-off-by: Roderick Colenbrander <roderick.colenbrander@sony.com>
> ---
>   drivers/hid/hid-playstation.c | 166 ++++++++++++++++++++++++++++++++++
>   1 file changed, 166 insertions(+)
> 
> diff --git a/drivers/hid/hid-playstation.c b/drivers/hid/hid-playstation.c
> index 64d6d736c435..ef8da272cf59 100644
> --- a/drivers/hid/hid-playstation.c
> +++ b/drivers/hid/hid-playstation.c
> @@ -32,9 +32,19 @@ struct ps_device {
>   	int (*parse_report)(struct ps_device *dev, struct hid_report *report, u8 *data, int size);
>   };
>   
> +/* Calibration data for playstation motion sensors. */
> +struct ps_calibration_data {
> +	int abs_code;
> +	short bias;
> +	int sens_numer;
> +	int sens_denom;
> +};
> +
>   #define DS_INPUT_REPORT_USB			0x01
>   #define DS_INPUT_REPORT_USB_SIZE		64
>   
> +#define DS_FEATURE_REPORT_CALIBRATION		0x05
> +#define DS_FEATURE_REPORT_CALIBRATION_SIZE	41
>   #define DS_FEATURE_REPORT_PAIRING_INFO		0x09
>   #define DS_FEATURE_REPORT_PAIRING_INFO_SIZE	20
>   
> @@ -68,13 +78,27 @@ struct ps_device {
>   #define DS_TOUCH_POINT_INACTIVE BIT(7)
>   
>   /* DualSense hardware limits */
> +#define DS_ACC_RES_PER_G	8192
> +#define DS_ACC_RANGE		(4*DS_ACC_RES_PER_G)
> +#define DS_GYRO_RES_PER_DEG_S	1024
> +#define DS_GYRO_RANGE		(2048*DS_GYRO_RES_PER_DEG_S)
>   #define DS_TOUCHPAD_WIDTH	1920
>   #define DS_TOUCHPAD_HEIGHT	1080
>   
>   struct dualsense {
>   	struct ps_device base;
>   	struct input_dev *gamepad;
> +	struct input_dev *sensors;
>   	struct input_dev *touchpad;
> +
> +	/* Calibration data for accelerometer and gyroscope. */
> +	struct ps_calibration_data accel_calib_data[3];
> +	struct ps_calibration_data gyro_calib_data[3];
> +
> +	/* Timestamp for sensor data */
> +	bool sensor_timestamp_initialized;
> +	uint32_t prev_sensor_timestamp;
> +	uint32_t sensor_timestamp_us;
>   };
>   
>   struct dualsense_touch_point {
> @@ -312,6 +336,96 @@ static struct input_dev *ps_touchpad_create(struct hid_device *hdev, int width,
>   	return touchpad;
>   }
>   
> +static int dualsense_get_calibration_data(struct dualsense *ds)
> +{
> +	short gyro_pitch_bias, gyro_pitch_plus, gyro_pitch_minus;
> +	short gyro_yaw_bias, gyro_yaw_plus, gyro_yaw_minus;
> +	short gyro_roll_bias, gyro_roll_plus, gyro_roll_minus;
> +	short gyro_speed_plus, gyro_speed_minus;
> +	short acc_x_plus, acc_x_minus;
> +	short acc_y_plus, acc_y_minus;
> +	short acc_z_plus, acc_z_minus;
> +	int speed_2x;
> +	int range_2g;
> +	int ret = 0;
> +	uint8_t *buf;
> +
> +	buf = kzalloc(DS_FEATURE_REPORT_CALIBRATION_SIZE, GFP_KERNEL);
> +	if (!buf)
> +		return -ENOMEM;
> +
> +	ret = ps_get_report(ds->base.hdev, DS_FEATURE_REPORT_CALIBRATION, buf,
> +			DS_FEATURE_REPORT_CALIBRATION_SIZE);
> +	if (ret) {
> +		hid_err(ds->base.hdev, "Failed to retrieve DualSense calibration info: %d\n", ret);
> +		goto err_free;
> +	}
> +
> +	gyro_pitch_bias  = get_unaligned_le16(&buf[1]);
> +	gyro_yaw_bias    = get_unaligned_le16(&buf[3]);
> +	gyro_roll_bias   = get_unaligned_le16(&buf[5]);
> +	gyro_pitch_plus  = get_unaligned_le16(&buf[7]);
> +	gyro_pitch_minus = get_unaligned_le16(&buf[9]);
> +	gyro_yaw_plus    = get_unaligned_le16(&buf[11]);
> +	gyro_yaw_minus   = get_unaligned_le16(&buf[13]);
> +	gyro_roll_plus   = get_unaligned_le16(&buf[15]);
> +	gyro_roll_minus  = get_unaligned_le16(&buf[17]);
> +	gyro_speed_plus  = get_unaligned_le16(&buf[19]);
> +	gyro_speed_minus = get_unaligned_le16(&buf[21]);
> +	acc_x_plus       = get_unaligned_le16(&buf[23]);
> +	acc_x_minus      = get_unaligned_le16(&buf[25]);
> +	acc_y_plus       = get_unaligned_le16(&buf[27]);
> +	acc_y_minus      = get_unaligned_le16(&buf[29]);
> +	acc_z_plus       = get_unaligned_le16(&buf[31]);
> +	acc_z_minus      = get_unaligned_le16(&buf[33]);
> +
> +	/*
> +	 * Set gyroscope calibration and normalization parameters.
> +	 * Data values will be normalized to 1/DS_GYRO_RES_PER_DEG_S degree/s.
> +	 */
> +	speed_2x = (gyro_speed_plus + gyro_speed_minus);
> +	ds->gyro_calib_data[0].abs_code = ABS_RX;
> +	ds->gyro_calib_data[0].bias = gyro_pitch_bias;
> +	ds->gyro_calib_data[0].sens_numer = speed_2x*DS_GYRO_RES_PER_DEG_S;
> +	ds->gyro_calib_data[0].sens_denom = gyro_pitch_plus - gyro_pitch_minus;
> +
> +	ds->gyro_calib_data[1].abs_code = ABS_RY;
> +	ds->gyro_calib_data[1].bias = gyro_yaw_bias;
> +	ds->gyro_calib_data[1].sens_numer = speed_2x*DS_GYRO_RES_PER_DEG_S;
> +	ds->gyro_calib_data[1].sens_denom = gyro_yaw_plus - gyro_yaw_minus;
> +
> +	ds->gyro_calib_data[2].abs_code = ABS_RZ;
> +	ds->gyro_calib_data[2].bias = gyro_roll_bias;
> +	ds->gyro_calib_data[2].sens_numer = speed_2x*DS_GYRO_RES_PER_DEG_S;
> +	ds->gyro_calib_data[2].sens_denom = gyro_roll_plus - gyro_roll_minus;
> +
> +	/*
> +	 * Set accelerometer calibration and normalization parameters.
> +	 * Data values will be normalized to 1/DS_ACC_RES_PER_G g.
> +	 */
> +	range_2g = acc_x_plus - acc_x_minus;
> +	ds->accel_calib_data[0].abs_code = ABS_X;
> +	ds->accel_calib_data[0].bias = acc_x_plus - range_2g / 2;
> +	ds->accel_calib_data[0].sens_numer = 2*DS_ACC_RES_PER_G;
> +	ds->accel_calib_data[0].sens_denom = range_2g;
> +
> +	range_2g = acc_y_plus - acc_y_minus;
> +	ds->accel_calib_data[1].abs_code = ABS_Y;
> +	ds->accel_calib_data[1].bias = acc_y_plus - range_2g / 2;
> +	ds->accel_calib_data[1].sens_numer = 2*DS_ACC_RES_PER_G;
> +	ds->accel_calib_data[1].sens_denom = range_2g;
> +
> +	range_2g = acc_z_plus - acc_z_minus;
> +	ds->accel_calib_data[2].abs_code = ABS_Z;
> +	ds->accel_calib_data[2].bias = acc_z_plus - range_2g / 2;
> +	ds->accel_calib_data[2].sens_numer = 2*DS_ACC_RES_PER_G;
> +	ds->accel_calib_data[2].sens_denom = range_2g;
> +
> +err_free:
> +	kfree(buf);
> +	return ret;
> +}
> +
>   static int dualsense_get_mac_address(struct dualsense *ds)
>   {
>   	uint8_t *buf;
> @@ -343,6 +457,7 @@ static int dualsense_parse_report(struct ps_device *ps_dev, struct hid_report *r
>   	struct dualsense_input_report *ds_report;
>   	uint8_t battery_data, battery_capacity, charging_status, value;
>   	int battery_status;
> +	uint32_t sensor_timestamp;
>   	unsigned long flags;
>   	int i;
>   
> @@ -387,6 +502,44 @@ static int dualsense_parse_report(struct ps_device *ps_dev, struct hid_report *r
>   	input_report_key(ds->gamepad, BTN_MODE,   ds_report->buttons[2] & DS_BUTTONS2_PS_HOME);
>   	input_sync(ds->gamepad);
>   
> +	/* Parse and calibrate gyroscope data. */
> +	for (i = 0; i < ARRAY_SIZE(ds_report->gyro); i++) {
> +		int raw_data = (short)le16_to_cpu(ds_report->gyro[i]);
> +		int calib_data = mult_frac(ds->gyro_calib_data[i].sens_numer,
> +					   raw_data - ds->gyro_calib_data[i].bias,
> +					   ds->gyro_calib_data[i].sens_denom);
> +
> +		input_report_abs(ds->sensors, ds->gyro_calib_data[i].abs_code, calib_data);
> +	}
> +
> +	/* Parse and calibrate accelerometer data. */
> +	for (i = 0; i < ARRAY_SIZE(ds_report->accel); i++) {
> +		int raw_data = (short)le16_to_cpu(ds_report->accel[i]);
> +		int calib_data = mult_frac(ds->accel_calib_data[i].sens_numer,
> +					   raw_data - ds->accel_calib_data[i].bias,
> +					   ds->accel_calib_data[i].sens_denom);
> +
> +		input_report_abs(ds->sensors, ds->accel_calib_data[i].abs_code, calib_data);
> +	}
> +
> +	/* Convert timestamp (in 0.33us unit) to timestamp_us */
> +	sensor_timestamp = le32_to_cpu(ds_report->sensor_timestamp);
> +	if (!ds->sensor_timestamp_initialized) {
> +		ds->sensor_timestamp_us = DIV_ROUND_CLOSEST(sensor_timestamp, 3);
> +		ds->sensor_timestamp_initialized = true;
> +	} else {
> +		uint32_t delta;
> +
> +		if (ds->prev_sensor_timestamp > sensor_timestamp)
> +			delta = (U32_MAX - ds->prev_sensor_timestamp + sensor_timestamp + 1);
> +		else
> +			delta = sensor_timestamp - ds->prev_sensor_timestamp;
> +		ds->sensor_timestamp_us += DIV_ROUND_CLOSEST(delta, 3);
> +	}
> +	ds->prev_sensor_timestamp = sensor_timestamp;
> +	input_event(ds->sensors, EV_MSC, MSC_TIMESTAMP, ds->sensor_timestamp_us);
> +	input_sync(ds->sensors);
> +
>   	for (i = 0; i < ARRAY_SIZE(ds_report->points); i++) {
>   		struct dualsense_touch_point *point = &ds_report->points[i];
>   		bool active = (point->contact & DS_TOUCH_POINT_INACTIVE) ? false : true;
> @@ -476,12 +629,25 @@ static struct ps_device *dualsense_create(struct hid_device *hdev)
>   	}
>   	snprintf(hdev->uniq, sizeof(hdev->uniq), "%pMR", ds->base.mac_address);
>   
> +	ret = dualsense_get_calibration_data(ds);
> +	if (ret) {
> +		hid_err(hdev, "Failed to get calibration data from DualSense\n");
> +		goto err;
> +	}
> +
>   	ds->gamepad = ps_gamepad_create(hdev);
>   	if (IS_ERR(ds->gamepad)) {
>   		ret = PTR_ERR(ds->gamepad);
>   		goto err;
>   	}
>   
> +	ds->sensors = ps_sensors_create(hdev, DS_ACC_RANGE, DS_ACC_RES_PER_G,

Looks like you got a bad rebase here. I have the following complain when
compiling this series up to this patch:

drivers/hid/hid-playstation.c: In function 'dualsense_create':
drivers/hid/hid-playstation.c:644:16: error: implicit declaration of function 'ps_sensors_create' [-Werror=implicit-function-declaration]
   644 |  ds->sensors = ps_sensors_create(hdev, DS_ACC_RANGE, DS_ACC_RES_PER_G,
       |                ^~~~~~~~~~~~~~~~~
drivers/hid/hid-playstation.c:644:14: warning: assignment to 'struct input_dev *' from 'int' makes pointer from integer without a cast [-Wint-conversion]
   644 |  ds->sensors = ps_sensors_create(hdev, DS_ACC_RANGE, DS_ACC_RES_PER_G,
       |              ^


The function ps_sensors_create() gets added in "HID: playstation: add
DualSense lightbar support", which seems like a mistake.

Cheers,
Benjamin

> +			DS_GYRO_RANGE, DS_GYRO_RES_PER_DEG_S);
> +	if (IS_ERR(ds->sensors)) {
> +		ret = PTR_ERR(ds->sensors);
> +		goto err;
> +	}
> +
>   	ds->touchpad = ps_touchpad_create(hdev, DS_TOUCHPAD_WIDTH, DS_TOUCHPAD_HEIGHT, 2);
>   	if (IS_ERR(ds->touchpad)) {
>   		ret = PTR_ERR(ds->touchpad);
>